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内 容 简 介 


非 高 斯 系统 估计 的 文献 一 般 都 存在 缺少 实用 方法 的 缺点 ， 而 高 斯 方法 又 缺乏 严密 、 有 组 织 的 推导 。 
《 贝 叶 斯 估计 与 跟踪 实用 指南 》 一 书 设 法 解决 这 个 领域 中 的 不 足 ， 为 读者 提供 全 面 的 高 斯 噪声 与 非 高 斯 
噪声 驱动 的 线性 与 非 线性 动态 系统 估计 方法 的 介绍 


本 书 特点 在 于 提供 了 贝 叶 斯 估计 与 跟踪 的 统一 方法 ， 重 点 强调 了 在 贝 叶 斯 框架 下 所 有 跟踪 算法 的 推导 
过 程 ， 还 给 出 了 用 于 计算 加 权 密 度 积分 的 高 效 的 数值 方法 ， 包 括 加 权 高 斯 积分 的 线性 与 非 线性 卡尔 曼 滤 波 
器 ， 以 及 非 高 斯 情况 下 的 粒子 滤波 器 。 作 者 首先 给 出 每 种 估计 方法 基本 原理 的 详细 推导 过 程 ， 然 后 分 步骤 
对 每 一 种 方法 进行 详细 说 明 ， 使 得 跟踪 滤波 器 的 编码 更 简单 易 懂 。 


本 书 利用 了 案例 分 析 来 展示 所 研究 内 容 的 应 用 。 此 外 ， 该 书 还 为 每 个 算法 提供 了 框图 ， 让 读者 可 以 自 
己 开发 MATLAB® 估 计 方 法 软件 工具 箱 . 


《 贝 叶 斯 估计 与 跟踪 实用 指南 》 是 一 本 优秀 的 估计 与 跟踪 方法 课程 的 研究 生 教材 。 本 书 还 适用 于 信 
计 与 跟踪 领域 的 科学 研究 人 员 、 数 学 研究 人 员 和 工程 技术 人 员 ， 对 他 们 具有 重要 的 实际 应 用 价值 。 对 广大 
科技 工作 者 来 说 ， 也 是 一 本 非常 有 价值 的 参考 书 。 


作者 简介 


安 东 .J. 霍 格 ， 博 士 ， 美 国 约翰 . 霍 普 金 斯 大 学 应 用 物理 实验 室 科 研 人 员 ， 他 为 防空 和 导弹 防御 系 
( AMDD ) 提出 了 先进 的 目标 跟踪 方法 。 和 霍 格 博士 的 研究 领域 包括 目标 跟踪 ， 信 号 和 阵列 处 理 与 处 理 
设计 ， 主 动 、 被 动 雷达 和 声 纳 设 计 ， 数 字 通 信和 编码 理论 以 及 时 间 频 率 分 析 。 





责任 编辑 : 牛 旭 东 xdniu@ndip.cn 
责任 校对 : 苏 向 颖 > Leu. HM < 


封面 设计 : BEF 

Copies of this book sold without 
a Wiley sticker on the cover are 
unauthorized and illegal 


一 定价 : 79.00 元 79.00 元 


= 


装备 科技 译 著 出 版 基金 


贝 叶 斯 估计 与 跟 踊 实用 指南 


Bayesian Estimation and Tracking:A Practical Guide 


[ 美 ] Anton J. Hauge = 
XR TB E 


A É - 4 ; É x 
. 北京 ， 


著作 权 合 同 登记 图 字 : 军 -2013 -200 = 


图 书 在 版 编目 ( CIP) 数据 


贝 叶 斯 估计 与 跟踪 实用 指南 / (Xt 
(Hang,A. J. ) 著 ; 王 欣 ,于 晓 译 . 一 北京 : 国防 工业 
出 版 社 ,2014. 5 

(高 新 科技 译 从 ) 

书 名 原文 :Bayesian estimation and tracking:a 
practical guide 

ISBN 978 -7—118 -09321 -6 


.DD 贝 … I. 四 豪 … OE OF- H. 中 贝 叶 斯 
估计 - 指南 名 贝 叶 斯 估计 -应 用 - 目标 跟踪 — 指南 IV. 
(D 0211. 67 -62®TN953 - 62 


中 国 版 本 图 书馆 CIP 数据 核 字 (2014 ) 第 068301 号 


Authorized translation from the English language edition, entitled Bayesian Estimation and 
Tracking, ISBN 978 -0 -470 -62170 -7, by Anton J. Haug, published by John Wiley & Sons . 

Copyright © 2012 by John Wiley & Sons, Inc. 

All rights reserved. This translation published under license. No part of this book may be repro- 
duced in any form without the written permission of the original copyrights holder. 


Copies of this book sold without a Wiley sticker on the cover are unauthorized and illegal. 
本 书简 体 中 文 版 由 John Wiley & Sons, Inc. 授权 国防 工业 出 版 社 独家 出 版 。 版 权 所 有 ， 
侵权 必 究 。 


A É - Ë ; Y 社 出 版 发 行 
(北京 市 海淀 区 紫竹 院 南路 23 号 ”邮政 编码 100048) 
北京 嘉 恒 彩 色 印刷 有 限 责任 公司 
新 华 书 店 经 售 


开本 710 x1000 1/16 印张 19 对 字数 370 TE 
2014 年 5 月 第 1 版 第 1 次 印刷 印 数 1 一 2000 册 定价 79. 00 元 


(本 书 如 有 印 装 错误 ,我 社 负责 调换 ) 


国防 书店 : (010)88540777 发 行 邮购 :(010)88540776 
发 行 传真 :(010)88540755 发 行业 务 :(010)88540717 


Ill 


BY 


本 书 以 基本 原理 为 出 发 点 ,阐述 了 贝 叶 斯 估计 滤波 器 的 完整 的 发 展 历程 。 
书 中 研究 了 由 高 斯 噪声 和 非 高 斯 噪声 驱动 的 线性 、 非 线性 动态 系统 。 这 里 假设 
动态 系统 是 连续 的 ,因为 只 有 在 研究 离散 滤波 器 的 时 候 , 才 会 在 离散 时 间 对 系统 
进行 观测 。 本 书 的 主要 目标 是 给 读者 提供 过 去 60 年 来 所 提出 的 大 部 分 贝 叶 斯 
估计 方法 的 一 致 性 的 全 面 描述 ,以 表明 每 种 以 贝 叶 斯 范式 为 基础 的 基本 思想 是 
如 何 与 条 件 密度 相关 的 。 

学 习 本 书 内 容 需 要 的 预备 知识 包括 线性 代数 、 贝 叶 斯 概率 理论 以 及 有 限 差 
分 和 插值 的 数值 方法 。 书 中 第 2 章 介 绍 了 所 有 这 些 主题 以 及 掌握 书 中 其 它 内 容 
所 需 的 基础 知识 。 

书 中 涵盖 的 部 分 内 容 来 自 于 马里 兰 大 学 帕克 分 校 数学 系 的 一 门 研究 生 课 
程 。 该 门 课程 的 主要 目的 是 让 学 生 自 己 开 发 目标 跟踪 算法 的 MATLAB 工具 箱 。 
课 后 给 学 生 们 提出 一 些 特定 的 跟踪 问题 ,要 求 编写 一 个 或 多 个 MATLAB 子 程序 
来 实现 某 些 跟 踪 方法 。 一 般 来 说 ,学 生 开 发 的 子 程序 是 独立 的 ,并 且 可 以 应 用 于 
多 种 难度 不 大 的 跟踪 问题 ( 例如, 纯 方位 跟踪 ) 。 本 书 不 包含 任何 习题 集 , 使 用 
本 书 的 读者 可 以 根据 需要 来 调整 跟踪 问题 的 作业 。 此 外 ,本 书包 含 4 个 相当 复 
杂 的 实用 案例 ,使 用 本 书 的 老师 可 以 选择 其 中 之 一 作为 编码 作业 的 框架 。 第 一 
个 案例 贯穿 本 书 的 第 工 和 第 下 部 分 ;其 余 的 3 个 案例 则 被 分 成 了 单独 的 章节 , 构 
成 了 本 书 的 第 V 部 分 。 

本 书 有 两 个 鲜明 特点 :中 运用 大 量 的 表格 和 图 形 ,给 出 每 种 估计 方法 基本 原 
理 的 详细 推导 过 程 ;@@ 每 种 方法 都 有 非常 详细 的 分 步 指导 ,使 得 跟踪 滤波 器 的 编 
码 简 单 且 易于 理解 。 

递 推 贝 叶 斯 估计 可 以 求解 转移 函数 或 变换 函数 的 加 权 条 件 密度 积分 。 转 移 
函数 反映 动态 状态 向 量 随时 间 变 化 的 路 径 ,变换 函数 将 状态 向 量变 换 成 观测 向 
量 。 有 多 种 求解 积分 的 数值 方法 ,每 种 都 有 不 同 的 估计 方法 。 本 书 第 开 部 分 和 
第 亚 部 分 的 每 个 章节 都 涵盖 求解 这 些 积 分 的 一 个 或 多 个 数值 逼近 方法 ,第 工 部 
分 是 高 斯 加 权 积 分 的 卡尔 曼 滤 波 器 方法 ,第 亚 部 分 是 未 知 密度 的 加 权 密 度 积分 
的 粒子 滤波 器 方法 。 

III 


本 书 是 作者 在 此 领域 多 年 研究 的 成 果 , 希 望 能 为 贝 叶 斯 估计 和 跟踪 研究 做 
出 贡献 。 同 时 ,也 希望 在 研究 改进 这 些 方 法 并 将 其 应 用 到 新 领域 解决 新 问题 的 
基础 上 ,本 书 能 够 开拓 贝 叶 斯 估计 和 跟踪 研究 的 新 方向 。 


Anton J. Haug 
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第 工 部 分 


预备 知识 


简介 

数学 基础 

贝 叶 斯 估计 的 基本 概念 
实用 案例 :初步 探讨 








自 统计 数学 出 现 以 来 ,动态 系统 估计 与 跟踪 已 成 为 许多 数学 家 的 研究 热点 。 
过 去 50 年 中 已 提出 许多 估计 方法 ,用 于 对 线性 和 高 斯 分 布 动态 系统 进行 统计 推 
理 ( 估 计 ) 。 对 于 非 线 性 高 斯 系统 的 应 用 ,有 些 方法 以 增加 计算 复杂 度 为 代价 ,也 
得 了 很 好 的 估计 结果 。 然 而 ,现实 世界 的 动态 系统 中 ,无 论 是 线性 的 还 是 非 线性 
的 ,通常 有 异常 值 过 多 的 现象 ,并 呈现 为 非 高 斯 分 布 ,已 经 证 明 , 在 处 理 这 种 真实 
世界 数据 的 时 候 , 常 规 的 高 斯 估计 方法 不 足以 处 理 这 些 问 题 ,会 导致 估计 滤波 器 
无 法 收敛 。 

随 着 高 速 桌面 计算 的 出 现 , 在 过 去 的 十 年 中 ,数学 研究 的 重点 已 经 转移 到 非 
高 斯 分 布 动态 系统 之 上 。 但 是 ,大 部 分 的 非 高 斯 系统 估计 相关 研究 成 果 都 是 理论 
上 的 , 那 种 普通 工程 人 员 无 需 太 多 学 习 就 可 以 方便 使 用 的 方法 基本 没有 -已 提出 
的 用 于 非 线性 系统 估计 的 高 斯 方法 ,都 缺少 严密 、 有 组 织 的 推导 ,在 概念 上 ,高 斯 
方法 和 非 高 斯 方法 仍然 缺乏 连续 性 。 

本 书 致力 于 将 最 新 的 动态 系统 统计 估计 方法 的 研究 成 果 全 面 地 呈现 给 读 
者 ,包括 线性 \ 非 线性 系统 和 高 斯 \ 非 高 斯 的 系统 等 ,利用 贝 叶 斯 框架 ,给 出 一 个 
概念 上 紧密 相关 的 路 线 图 。 首 先 ,从 基本 原理 开始 介绍 ,进而 引出 一 些 目前 广泛 
使 用 的 高 斯 估计 方法 ;然后 ,将 这 些 概念 推广 到 非 高 斯 估计 领域 ,并 采用 蒙特 卡 
洛 方法 进行 估计 。 虽 然 贝 叶 斯 方法 可 求 得 统计 密度 估计 ,但 多 数 情况 下 使 用 点 估 
计 方 法 ,计算 加 权 密 度 积分 并 得 统计 密度 估计 。 因 此 ,本 书 主要 介绍 在 高 斯 、 非 高 
斯 密度 下 的 加 权 密 度 积分 数值 计算 方法 。 

本 书 还 为 每 种 估计 算法 提供 了 伪 代 码 和 框图 , 供 读者 开发 跟踪 软件 工具 箱 。 
为 帮助 读者 理解 书 中 所 给 出 的 算法 ,本 书 还 将 针对 几 个 非常 具体 的 实际 问题 , 介 
绍 一 些 开发 仿真 所 需要 的 知识 。 书 中 将 给 出 案例 分 析 ,并 给 出 每 种 跟踪 估计 的 结 
果 。 因 为 很 难 专门 地 去 对 跟踪 方法 进行 比较 ,所 以 本 书 给 出 了 多 种 评价 方法 ,用 
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于 评价 各 种 跟踪 滤波 器 的 相对 性 能 。 


1.1 贝 叶 斯 推理 


推理 方法 是 对 一 组 参数 估计 出 当前 值 的 过 程 ,这 组 参数 基于 一 组 观察 值 或 
测量 值 , 估 计 过 程 一 般 遵 循 以 下 两 个 模型 ,第 一 个 模型 假定 待 估计 参数 (通常 是 
不 可 观测 的 ) 是 非 随机 的 并 且 在 观察 窗 中 保持 恒定 ,但 是 观测 值 含 有 噪声 和 随 
机 分 量 ; 第 二 个 模型 假定 参数 是 随机 变量 ,具有 一 定 先 验 概率 , 且 观 测 含 噪 。 利 用 
第 一 个 模型 进行 参数 估计 的 过 程 被 称 为 非 贝 叶 斯 估计 或 Fisher 估计 '" ,利用 第 
二 个 模型 进行 参数 估计 被 称 为 贝 叶 斯 估计 。 

贝 叶 斯 估计 的 概念 非常 简单 , 它 基 于 一 些 初始 先 验 信 念 。 例 如 : “TE A 
船 。 它 离 海岸 914m(1000yd) ,向 东北 方向 以 10n mile/h 的 速度 运动 。” 初 始 信念 
的 陈述 (包括 最 初 猜测 的 船舶 的 位 置 和 速度 ) 是 不 确定 或 随机 的 , 且 基 于 某 些 先 
验 的 概率 分 布 。 基 于 初始 信念 ,可 以 做 出 如 下 预测 :由 于 船舶 以 恒定 速度 运动 , 它 
将 在 大 约 10min 之 后 到 达 那 个 地 方 。 这 个 声明 包括 船舶 运动 动力 学 概念 模型 以 
及 一 些 不 确定 性 。 现 在 假设 ,一 个 人 手 上 有 一 个 小 的 便携 式 雷 达 , 该 雷达 能 够 以 
一 定 的 精度 测量 (观测 ) 船舶 视线 范围 内 的 距离 和 速度 ,给 定 适当 的 数学 模型 ， 
可 将 第 卡 儿 坐 标 系 下 观测 到 的 船舶 位 置 和 速度 联系 起 来 ,当前 的 雷达 测量 值 可 
以 用 来 更 新 预测 船舶 状态 (位 置 与 速度 ) 。 

上 面 一 段 阐述 了 递归 贝 叶 斯 估计 的 实质 : 

(1) 给 出 初始 信念 陈述 ; 

(2) 利用 先 验 信念 和 动态 模型 进行 预测 ; 

(3) 利用 观测 值 和 观测 模型 来 得 后 验 信念 ,更 新 预测 值 ; 

(4) 将 后 验 信念 置 成 新 的 先 验 信念 ,返回 步骤 (2) 。 

递 推 估计 的 概念 最 初 是 在 神甫 托马斯 * 贝 叶 斯 的 一 篇 论文 中 首次 提出 的 ， 
而 在 贝 叶 斯 去 世 数 年 之 后 (1763 年 ) ,由 理 查 德 * 普 莱 斯 投稿 到 皇家 统计 学 会 。 
在 Stephen Brooks"! 的 论文 中 ,可 以 读 到 与 贝 叶 斯 统计 推理 有 关 的 历史 和 思想 
的 精彩 回顾 。Brooks 的 论文 中 还 有 一 些 值得 关注 的 例子 ,例如 进行 了 贝 叶 斯 方法 
与 所 谓 “ 频 率 论 者 ”的 统计 推理 方法 之 间 的 对 比 , 因 为 本 书 致力 于 研究 贝 叶 斯 方 
法 ,不 再 深入 探讨 频率 论 的 方法 , 感 兴趣 的 读者 可 参考 Brooks 的 论文 。 


1.2. 贝 叶 斯 层次 估计 法 


如 上 所 述 ,本 书 将 对 现代 跟踪 滤波 器 中 一 个 子 集 进 行 严密 有 组 织 的 推导 。 
图 1 -1 给 出 了 本 书 提出 的 跟踪 滤波 器 层次 结构 。 

图 1 - 1 左边 是 高 斯 跟踪 滤波 器 ,右边 是 蒙特 卡 洛 非 高 斯 滤波 器 。 在 后 续 章 
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节 对 每 种 跟踪 滤波 器 进行 详细 阐述 的 过 程 中 ,将 以 此 图 作为 向 导 , 通 过 它 来 定位 
学 习 的 进程 , 书 中 还 会 介绍 其 他 一 些 感 兴趣 的 概念 ,如 讨论 如 何 评价 性 能 等 ,在 
多 数 情况 下 , 书 中 将 按照 从 上 到 下 、 由 左 至 右 的 方式 ,对 框图 内 容 加 以 介绍 ,在 
图 1 -1 中 右 下 角 内 容 介 绍 完毕 之 后 ,读者 将 会 对 所 有 的 贝 叶 斯 跟踪 滤波 器 之 
间 的 关联 有 一 个 全 面 的 认识 。 

贝 叶 斯 跟踪 器 


高 斯 跟踪 器 蒙特 卡 洛 跟踪 器 
线性 高 斯 跟踪 器 非 线性 高 斯 跟踪 器 序 贯 粒子 滤波 器 


线性 化 下 波 器 数值 积分 PEN | 高 斯 粒 
要 性 采 | Hat 
样 粒子 | | 器 
l [ubi 28 


线性 卡 || 扩 展 卡 | 高 斯 厄 || 无 迹 卡 || 球 面 单 | 蒙特 
ARB || AR 89 QE | KEE || AR BE pants 洛 卡尔 广义 组 
波 器 || 波 器 P. 波 器 awu Bux 合 粒子 


器 滤波 器 
有 限 差 
tf 
举 FH 
: 
as 器 a 
图 1 -1 贝 叶 斯 估计 的 跟踪 滤波 器 的 层次 结构 
1.3 本 书 研究 范畴 


1.3.1 目标 


本 书 主要 目的 是 让 读者 能 够 清晰 地 了 解 各 种 贝 叶 斯 估计 方法 及 其 相互 关 
系 , 首 先 从 贝 叶 斯 理论 的 基本 原理 开始 介绍 ,并 在 先前 的 滤波 器 基础 上 ,通过 细 
微 的 改进 推导 出 各 种 跟踪 滤波 器 ,这 种 推导 方式 可 以 让 读者 更 深入 地 理解 贝 叶 
斯 估计 与 跟踪 算法 的 层次 关系 ,在 介绍 跟踪 滤波 器 的 过 程 中 ,为 每 种 滤波 器 提供 
了 伪 代 码 和 程序 流程 图 ,以 方便 读者 查询 及 参考 。 

在 Jazwinski 最 早出 版 于 1970 年 的 关于 滤波 理论 的 书 中 ,有 如 下 论述 : 
“本 书 的 意义 是 双重 的 ,首先 ,尽管 线性 估计 理论 广为人知 ,但 多 数 成 果 都 分 散 
在 期 刊 文献 当中 ,没有 被 搜集 在 一 起 ;其 次 , 现 有 文献 在 连续 非 线 性 理论 上 的 研 
究 非常 深奥 争议 很 大 。 因 此 ,如 果 对 测度 理论 和 随机 微分 方程 没有 深入 了 解 , 普 
通 工 程 师 很 难 将 这 些 理论 应 用 到 实际 当中 。” 类 似 的 论述 也 适用 于 估计 理论 中 
非 高 斯 蒙特 卡 洛 算法 的 研究 现状 。 大 多 数 已 发 表 的 成 果 都 非常 深奥 ,对 测度 理论 
不 了 解 的 普通 工程 师 无 法 将 其 应 用 到 实践 中 .Doucet 等 ” 的 特 邀 论文 集 就 是 一 
个 典型 的 例子 ,在 这 部 优秀 的 特 邀 论文 集中 ,许多 章节 都 非常 深奥 。 


本 书 将 沿用 Jazwinski 的 方法 , 删 减 过 多 的 基于 测度 理论 的 阐述 ,普通 工程 
师 很 难 理解 这 些 深奥 的 数学 知识 ,希望 本 书 没有 采用 同样 深奥 的 数学 理论 来 蔡 
代 它 们 。 


13.2 章节 概述 


本 书 不 是 一 本 入 门 级 的 教科 书 , 其 编写 目的 是 作为 一 门 研究 生 课 程 ,或 者 给 
希望 深入 学 习 跟 踪 估计 算法 的 读者 提供 参考 。 本 书 的 读者 需要 对 概率 论 相 关 的 
书籍 (如 Papoulis 1) 达到 研究 生 水 平 的 理解 .同时 ,读者 还 应 该 熟练 掌握 矩阵 
线性 代数 和 数值 方法 ,包括 有 限 差分 ,为 减少 读者 学 习 本 书 的 困难 ,第 2 章 介绍 
了 贯穿 全 书 的 数学 工具 。 对 这 部 分 内 容 可 以 有 选择 性 地 阅读 ,有 经 验 的 读者 可 以 
跳 过 这 章 , 对 于 学 习 掌握 第 H HL 部 分 内 容 没 有 影响 。 

第 D 部 分 分 为 4 章 。 第 2 章 概括 介绍 了 学 习 估 计算 法 所 需要 的 数学 理论 。 第 
3 章 介绍 了 动态 系统 贝 叶 斯 递归 估计 的 概念 ,该 系统 可 以 用 离散 隐 式 马尔 可 夫 
过 程 进行 建 模 。 在 观测 模型 中 ,观测 值 为 离散 的 ,与 系统 状态 相关 。 本 书 一 般 不 考 
虑 连续 估计 算法 。 第 4 章 是 案例 分 析 一 一 通过 定向 频率 分 析 与 记录 (DIFAR) 声 
纳 浮 标 得 分 布 场 ,进行 船舶 跟踪 。 案 例 分 析 中 包括 给 出 系统 动力 学 的 全 仿真 和 生 
成 含 噪 观测 值 等 。 

第 H 部 分 涵盖 了 高 斯 噪声 情况 下 估计 方法 的 提出 与 应 用 ,第 5 章 用 高 斯 概 
率 密度 对 第 3 章 提出 的 一 般 贝 叶 斯 估计 方法 进行 了 修改 。 在 第 6 章 ~ 第 12 章 ,对 
高 斯 卡尔 曼 滤 波 器 算法 进行 了 推导 和 代码 实现 ,包括 线性 卡尔 曼 滤波 器 (LKF ) 、 
扩展 卡尔 曼 滤波 器 (EKF) 有 限 差分 卡尔 曼 滤波 器 (FDKF) ,无 迹 卡尔 曼 滤波 器 
(UKF) \ 球 面 单 形 卡 尔 曼 滤波 器 (SSKF) 、 高 斯 厄 米 特 卡尔 曼 滤波 器 (GHKF) 和 
蒙特 卡 洛 卡尔 曼 滤波 器 (MCKF) . 除 MCKF 以 外 ,后 5 种 跟踪 滤波 器 中 的 4 种 都 
可 以 归 类 为 一 般 Sigma 点 卡尔 曼 滤波 器 , 即 : 利 用 确定 性 矢量 点 积分 来 计算 高 斯 
加 权 积 分 ,并 进一步 用 来 估计 状态 矢量 的 均值 和 协 方差 矩阵 。MCKF 用 采样 分 布 
代替 连续 高 斯 分 布 ,这 样 估计 积分 运算 就 简化 为 求 和 运算 ,保持 了 非 线性 函数 的 
特性 ,在 第 13 章 中 可 以 看 到 ,后 5 种 卡尔 曼 滤波 方法 可 以 概括 为 一 种 估计 方法 ， 
它们 的 区 别 仅仅 是 用 到 的 矢量 点 的 数量 、 位 置 和 权重 的 变化 。 

在 很 多 估计 算法 文献 中 ,常常 忽略 了 一 个 关键 问题 估计 算法 性 能 的 量 
化 测度 问题 ,本 书 第 14 章 的 主题 是 研究 算法 测度 问题 ,给 出 在 真实 系统 环境 下 ， 
基于 估计 协 方差 矩阵 计算 和 测绘 误差 椭圆 的 方法 ,以 及 蒙特 卡 洛 仿真 环境 下 , 计 
算 均 方 根 (RMS) RÆ Cramer - Rao 下 界 (CRLB) 的 方法 ,第 14 章 最 后 一 节 主 
要 研究 各 种 估计 方法 在 DIFAR 浮标 跟踪 中 的 应 用 ,以 及 降低 输入 信号 信 品 比 时 
的 性 能 对 比 结果 。 

第 H 部 分 详细 讨论 基于 非 高 斯 概率 分 布 的 估计 算法 。 在 第 12 章 中 ,采用 一 
组 离散 蒙特 卡 洛 采样 来 实现 高 斯 密度 估计 ,将 求 均值 . 求 协 方差 中 估计 积分 值 的 
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运算 简化 为 求 加 权 和 运算 ,分 别称 为 采样 均值 和 采样 协 方差 。 高 斯 密度 的 采样 权 
值 通 常 为 1/N, 这 里 N 代表 采样 点 数目 。 非 高 斯 密度 采样 有 如 下 两 个 问题 :GD 从 
密度 函数 中 直接 得 蒙特 卡 洛 采样 非常 困难 ;@) 当 密 度 函 数 不 存在 一 阶 矩 和 二 阶 
和 矩 的 时 候 , 采 样 会 出 现 问题 , 柯 西 密度 是 一 个 典型 的 例子 。 为 解决 上 述 采 样 问题 ， 
第 15 章 引 入 了 蒙特 卡 洛 方法 并 提出 了 重要 性 采样 的 概念 ,在 此 基础 上 推导 出 粒 
子 滤 波 器 估计 方法 (这 里 的 粒子 就 是 蒙特 卡 洛 采 样 点 ) ,在 粒子 滤波 器 使 用 过 程 
中 ,会 出 现 一 些 问题 ,为 解决 这 些 问 题 , 研 究 者 提出 了 很 多 改进 方法 .对 于 重要 性 
采样 ,每 个 采样 点 的 权 值 都 是 通过 计算 该 采样 点 的 非 高 斯 密度 与 重要 性 密度 的 
比值 得 的 。 在 一 定 的 假设 条 件 下 ,可 以 递归 地 计算 权重 ,这 样 就 产生 了 序 贯 重要 
性 采样 (SIS ) 粒子 滤波 器 ( 详 见 第 16 章 ) ,第 17 章 提出 在 滤波 器 每 一 步 迭 代 过 程 
中 重新 计算 权 值 ,由 此 产生 了 高 斯 组 合 粒子 滤波 器 。 在 第 17 章 的 末尾 ,还 给 出 
DIFAR 应 用 中 ,各 种 粒子 滤波 跟踪 估计 方法 性 能 的 比较 结果 。 

几 本 最 近 出 版 的 书籍 为 书 中 研究 的 主题 提供 了 一 些 有 益 的 参考 -其 中 ,在 学 
习 高 斯 卡尔 曼 滤波 器 时 ,Bar Shalom 等 ” 和 Candy" 的 文献 是 非常 好 的 指南 书 
籍 .而 对 于 学 习 第 HU 部 分 的 非 高 斯 滤波 方法 来 说 ,Doucet 等 和 Ristic 等 的 著 
作 则 是 非常 优秀 的 参考 书 。 


1.4 FR MATLAB® 进行 建 模 和 仿真 


学 习 过 程 非常 重要 的 是 能 够 为 读者 提供 一 些 具体 实例 ,来 实现 复杂 问题 的 
估计 ,本 书 通 过 使 用 MATLAB® 进行 仿真 来 保证 这 一 点 。 本 书 共 提 供 4 个 案例 分 
Er ,从 第 一 个 到 最 后 一 个 ,复杂 度 逐 渐 增 加 。 每 一 个 案例 分 析 中 ,首先 介绍 如 何 对 
系统 进行 动态 建 模仿 真 和 观测 建 模仿 真 ;然后 说 明 如 何 用 蒙特 卡 洛 方法 生成 随 
机 观测 数据 ;之 后 运用 各 种 跟踪 滤波 器 对 仿真 的 观测 数据 进行 处 理 , 产 生 多 组 跟 
踪 数 据 ; 最 后 用 跟踪 数据 对 各 种 跟踪 滤波 器 加 以 比较 。 

第 一 个 案例 研究 的 是 船舶 跟踪 问题 。 当 运动 船舶 穿 过 DIFAR 浮标 分 布 场 
时 ,对 它 进行 跟踪 ,DIFAR 浮标 利用 船舶 发 射 的 宽带 含 品 信号 作为 输入 ,将 得 的 
船舶 方位 含 品 观 测 值 作 为 输出 。 第 4 章 内 容 表明 ,DIFAR 浮标 输出 的 方位 估计 概 
率 密度 依赖 于 输入 信号 的 信 噪 比 , 即 : 信 噪 比 很 高 时 ,概率 密度 为 高 斯 分 布 , 随 着 
信 噪 比 不 断 减 小 ,概率 密度 会 逐渐 变 为 均匀 分 布 - 研 究 这 一 案例 的 目的 在 于 , 当 
观测 噪声 从 高 斯 分 布 变 为 非 高 斯 分 布 时 ,检测 每 种 跟踪 估计 算法 的 滤波 跟踪 
性 能 。 

本 书 利 用 DIFAR 实例 ,依次 说 明 各 种 跟踪 估计 滤波 器 ,第 4 章 将 介绍 如 何 实 
H DIFAR 浮标 的 仿真 , 以 产生 模拟 的 、 信 品 比 相关 的 观测 值 集合 ,在 后 续 章 节 
中 ,采用 上 述 方位 观测 值 集合 ,运用 各 种 跟踪 算法 来 产生 蒙特 卡 洛 跟踪 估计 集 
合 , 清 晰 展现 高 斯 观测 噪声 变 为 非 高 斯 观测 噪声 对 各 种 跟踪 方法 的 影响 。 
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第 IV 部 分 还 给 出 另外 三 个 案例 分 析 , 用 来 说 明 第 H 和 第 HL 部 分 介绍 的 跟 
踪 滤 波 器 的 使 用 方法 。 

第 18 章 主要 解决 如 何 实现 三 维 空间 机 动 目标 跟踪 这 一 关键 问题 .这 里 介绍 
了 一 种 新 方法 ,使 用 恒定 球面 速度 模型 来 取代 经 典 的 恒定 笛 卡 儿 速 度 模型 FF 
释 在 使 用 多 种 高 斯 跟踪 滤波 器 跟踪 机 动 目标 时 ,球面 模型 在 改善 跟踪 性 能 上 的 
表现 。 | 

第 19 章 给 出 了 第 3 个 案例 分 析 ,其 中 研究 了 相当 复杂 的 下 落 炸 弹 动态 跟踪 
问题 ,具体 是 使 用 多 跟踪 点 视频 帧 来 实现 跟踪 ,跟踪 点 包括 投下 炸弹 的 飞机 和 炸 
弹 本 身 。 这 是 一 种 被 称 为 摄影 测量 法 的 复杂 过 程 的 案例 ,对 象 的 几何 属性 和 运动 
学 属性 都 可 以 从 连续 拍摄 的 图 像 帧 中 推算 得 ,这 个 案例 中 包含 非常 复杂 的 非 线 
性 多 维 观 测 过 程 和 非 线 性 多 维 动力 学 模型 ,而 且 ,这 里 动力 学 模型 和 观测 模型 都 
是 高 维 的 ,是 跟踪 滤波 器 中 最 为 复杂 的 问题 ,这 里 通过 维度 的 影响 来 证 明 , 使 用 
所 有 的 跟踪 滤波 器 在 计算 上 是 不 可 行 的 。 

第 20 章 给 出 最 后 一 个 案例 分 析 , 改 进 了 摄影 测量 法 在 估计 中 的 应 用 ,使 用 
多 个 传感器 ,如 多 个 摄像 头 .平移 加 速度 计 和 角 速 率 陀螺 仪 等 ,将 多 传感器 采集 
到 的 数据 进行 融合 .这 些 数据 在 单独 使 用 时 ,每 种 数据 源 都 有 它 的 优势 ,但 也 都 
有 缺点 。 当 使 用 多 个 不 同 的 传感器 联合 估计 时 ,得 的 估计 结果 通常 更 为 准确 、 可 
靠 。 通 过 对 估计 结果 的 分 析 表 明 ,传感器 融合 辅助 估计 器 可 以 从 飞行 测试 中 恢复 
有 意义 的 结果 ,否则 测试 就 会 失败 。 
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第 2 章 
数学 基础 





本 章 将 介绍 一 些 数学 概念 ,它们 是 理解 第 H 至 N 部 分 内 容 必 不 可 少 的 知 
识 。 这 里 仅 将 对 相关 数学 知识 进行 简要 介绍 ,而 不 会 涉及 大 量 的 严格 的 证 明 。 对 
于 这 里 所 涵盖 的 大 多 数 主题 ,将 假定 读者 很 熟悉 之 前 提 到 的 数学 概念 .虽然 不 建 
iX ,但 读者 可 以 在 初次 阅读 的 时 候 跳 过 这 个 章节 ,在 阅读 第 ID 部 分 其 他 内 容 和 
第 EN 部 分 时 ,可 将 本 章 作 为 索引 使 用 .补充 的 其 它 数 学 概念 将 会 根据 需要 
加 以 介绍 。 


2.1 和 矩阵 线性 代数 简介 


本 节 给 出 本 书 中 所 用 到 的 符号 约定 和 矩阵 运算 ,并 利用 MATLAB® 环境 ， 
提供 一 些 矩 阵线 性 代数 概念 的 简要 概况 -在 线 的 参考 文献 [1] 对 于 本 节 介 绍 的 
和 矩阵 运算 和 公式 是 非常 好 的 参考 资料 。 


2.1.1 矢量 与 矩阵 的 约定 与 记 法 


通常 用 小 写字 母 来 表示 标量 ,例如 c; 用 小 写 的 黑 斜 体 字 母 来 表示 一 个 矢 
量 , 例 如 a; 用 大 写 的 黑 斜 体 字母 来 表示 和 矩阵 ,例如 4。 
— n A^ n x 1 的 列 矢 量 , 即 


ae (2-1) 
a, 


按照 惯例 ,所 有 的 矢量 都 是 列 矢量 ,可 以 将 列 矢 量 写成 行 撩 量 的 转 置 (在 报告 或 
期 刊 中 ,有 利于 节省 空间 ) , 即 

a = [a,,-*',a,]" (2 -2) 
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用 逗号 隔 开 矢 量 中 的 元 素 。[ : ]' 表示 一 个 矢量 或 者 矩阵 的 转 置 ,后 面 将 给 出 其 
定义 。 
— n x m 矩阵 可 表示 为 如 下 的 二 维 数组 , 即 


a, cU Qim 
e hi i (2 -3) 
a, ax A nm 


式 中 :第 一 个 下 标 是 元 素 所 在 行 的 索引 ;第 二 个 下 标 是 元 素 所 在 列 的 索引 。 和 矩阵 
AW ij PICK ajo 
用 符号 A' 表示 矩阵 4 的 转 置 ,定义 为 


(2 -4) 





n x n JIRE A 的 迹 被 定义 为 矩阵 4 的 主 对 角 线 (从 左上 到 右 下 的 对 角 线 ) 上 
元 素 的 和 , 即 
uA = Ya, (2-5) 


同时 ,和 矩阵 的 迹 也 是 其 特征 值 的 总 和 ,因此 其 具有 基 变 换 下 的 不 变性 .这 一 特性 
可 以 用 来 定义 一 般 的 线性 算 子 的 迹 。 

本 书 偶尔 也 会 使 用 符号 4 e R"" 或 者 a e R' 来 分 别 指定 矩阵 或 者 矢量 的 
维度 。 


2.1.2 “和 与 乘积 


矩阵 的 加 法 和 数 乘 定义 为 
C = aA +BB (2 -6) 
c; 7 aay + Bb, (i = leens j = 1 ,**…,m) (2 -7) 
两 个 矩阵 4 和 R” FAB e RR” 的 乘积 为 
C = AB (2 -8) 
应 注意 ,和 矩阵 4 的 列 数 必须 等 于 矩阵 B ITÄ EBE C e R"" 的 元 素 为 
k 
cy = * aub; (2 -9) 
两 个 相同 维 数 的 矩阵 的 Hadamard 积 是 二 者 对 应 位 置 元 素 的 乘积 , 即 
C = AGB (2 - 10) 
C; = a,b, (2 - 11) 


附注 2.1 在 MATLAB@ 环境 中 ,全 矩阵 乘法 表示 为 C = A * B, Hadamard 
积 表示 为 C = A* B. 
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2.1.3 和 矩阵 的 逆 


方 阵 4 AOR RE RE AW! 必须 满足 
AA" = AA = I (2 = 12) 
式 中 :7 是 与 矩阵 4 有 相同 维度 的 单位 矩阵 .单位 矩阵 是 主 对 角 线 上 的 元 素 为 1、 
其 余 元 素 均 为 0 的 方 阵 。 并 非 所 有 和 矩阵 都 有 逆 和 矩阵 。 

没有 逆 和 矩阵 的 矩阵 被 称 为 奇异 矩阵 。 逆 矩阵 的 所 有 元 素 包 含 一 个 矩阵 行列 
式 的 除数 。 判 断 一 个 矩阵 是 否 是 奇异 矩阵 ,一 个 简单 的 方法 是 计算 它 的 行列 式 。 
如 果 行 列 式 是 0 ,那么 矩阵 是 奇异 的 。 

可 以 有 多 种 方法 计算 首 和 矩阵 .对 于 小 型 矩阵 ,可 使 用 基于 代数 余子 式 和 和 抢 阵 
行列 式 的 解析 方法 进行 求解 。 TT RT FAKER, 采用 数值 方法 进行 求解 , 例如 
Gauss - Jordon 消 元 法 或 LU 分 解法 。 如 果 和 矩阵 为 对 称 正 定 和 矩阵 ,LU 分 解法 可 简 
化 为 Cholesky 分 解法 .具体 技术 细节 可 参考 关于 和 矩阵 线性 代数 的 文献 。 

附注 2.2 在 MATLAB@ 环境 中 ,有 两 个 求解 等 式 4*x =b 的 主要 方法 ， 
其 中 一 个 是 首选 的 。 另 一 种 不 那么 常用 , 但 更 简单 的 方法 是 直接 使 用 x = 
A^" *b:x = inv(A) * b,3x £ inv( A) 是 一 个 MATLAB@ 函数 ,解析 计算 4-, 运 
算 符 * 表示 矩阵 乘法 。 更 好 的 解决 方案 是 利用 矩阵 左 除 运算 或 者 反 斜 杠 :x = 
A\b。 后 者 使 用 高 效 的 LU 分 解法 或 者 Cholesky 分 解法 ,这 取决 于 和 矩阵 4 的 性 质 。 

需要 注意 的 是 , 求 逆 矩阵 时 ,可 以 用 B = Aleye(size(A)) 来 高 效 求 得 ,这 里 
BHA Wit E eye(N) 是 一 个 MATLAB@ 函数 ,可 以 用 来 得 除了 对 角 线 元 素 
为 1 外 ,其 他 元 素 都 为 0 的 单位 矩阵 ,MATLAB® 函数 eye 还 有 其 他 的 功能 ,例如 
可 以 生成 非 方 矩阵 ,这 里 不 再 详细 讨论 。 

如 果 和 矩阵 线性 代数 方程 是 xx*4 =b, Ax =b*A”, 和 MATLAB® 表达 
式 x = A/b 对 矩阵 进行 表示 。 类 似 的 ,MATLAB 根据 4 和 4b 的 属性 ,选取 最 优 的 算 
法 进行 计算 。 

最 后 ,如 果 只 需要 进行 两 个 相同 维 数 矩阵 的 对 应 位 置 元素 的 除法 ,可 以 用 
MATLAB? 的 C = A./B 表达 式 。 


2.1.4 分 块 矩阵 的 逆 


对 于 分 块 矩 阵 , 有 一 些 求 道 的 通用 公式 。 
引 理 2.1 MMPS | BE GE A CHAC? + D4 B) 是 非 奇 异 方 阵 , 那 么 


(4+BCD)- = A” -A"'B(C" + DA !B) 'DA (2 - 13) 
引 理 2.2 Spee Re wt n x m EE M 分 块 
M = [4 B | n 
io alga (2 - 14) 


式 中 :n x n Hike A 和 m x m IERE D JEn] AY BB 4" Be BU 


A ByX Y I, 0 
b ple at la pt Bo 
可 以 推 知 
[> jal T (2 -16) 
Z U C D 
PE mfi FF) y Ha kE [Ve 5 33 A 
- -| (A = BD CY- -A"B(D +C4-B)- 
spP °C = BD” GC)! (D +CA`'B) ` 
还 可 以 通过 下 式 计 算 分 块 矩 阵 的 逆 和 矩阵 
X YirA B I, 0 
Io wills si len (2 - 18) 
进而 得 相同 分 块 矩 阵 的 逆 和 矩阵 为 
pre -| (A-BD'C): = (A -BD'"C) BD" (3 - 19) 
= (D + CA” B) t CA" (D +CA'B)" 


2.1.5 和 矩阵 的 平方 根 


对 任意 非 奇 异 方 阵 A ,A 的 平方 根 用 和 矩阵 B 表示 ,满足 等 式 
A = BB' (2 - 20) 
它 实 际 上 不 是 平方 根 , 而 是 一 种 矩阵 分 解 。 

附注 2.3 ”在 MATLAB@ 中 有 一 个 特殊 的 函数 B = sqrim( A) , HF Rit KH 
阵 4 的 主 ( 正 ) 平方根 。 轨 是 每 个 特征 值 都 有 非 负 实 部 的 唯一 的 平方 根 。 如 果 4 有 
非 负 实 部 的 特征 值 ,那么 将 得 复数 解 。 如 果 人 4 是 奇异 的 ,那么 4 可 能 没有 平方 根 。 
如 果 检 测 到 确切 的 奇异 点 ,MATLAB@ 将 给 出 出 错 信 息 。 

另 一 种 方法 利用 Cholesky 分 解法 来 计算 下 三 角 和 矩阵 如 ,该 矩阵 有 满足 矩阵 
平方 根性 质 的 严格 正 对 角 线 元 素 ,B = chol(4) HERA 生成 一 个 上 三 角 和 矩阵 
B' ,满足 等 式 4 = BB .假设 下 三 角 和 矩阵 如 是 上 三 角 和 矩阵 B" (RHR) HE. 
矩阵 4 必须 是 正定 的 ;否则 MATLAB® 将 给 出 出 错 信息 。 

尽管 MATLAB@ 中 计算 矩阵 平方 根 的 不 同方 法 生成 不 同 的 结果 矩阵 B, 但 
它们 都 是 有 效 的 ,只 是 使 用 的 数值 技术 不 同 。 采 用 哪 种 方法 取决 于 具体 的 应 用 。 


2.2 矢量 点 发 生 器 


在 讨论 Sigma 点 卡尔 曼 滤 波 器 (第 8 ~ 第 12 章 ) 时 ,本 书 将 利用 一 个 简化 计 
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数 法 来 生成 撩 量 Sigma 点 ,进行 多 维 数值 积分 。 
定义 H RERE PROS 
u = (u,,"7,u,,0,*,0) e R" (3 213 
其 中 ,如 果 i<j[2,3], 则 0 < u, < u TEXX RIETI P LR" 指定 & 生成 矢量 点 的 
HER u; 表示 + ui。 这 种 发 生 器 可 以 表示 为 
[u] e R’ (2 = 22) 
或 者 [uu] e R" (2 — 23) 
式 中 :& A [uw ,…,u,] 为 方便 表达 ,隐藏 了 零 坐标 .这 种 计数 法 表示 的 上 维 矢量 
点 集合 ,可 以 从 对 u 进行 置换 或 交换 阁 干 坐标 记号 来 产生 。 
在 第 H 部 分 ,将 研究 高 斯 加 权 积分 的 多 维 积分 规则 ,数值 解法 涉及 矢量 点 
集 非 线性 函数 的 评价 ,包括 一 个 常数 乘 以 单位 超 球面 或 超 立 方 体 上 的 矢量 点 集 
合 。 因 此 ,本 书 会 利用 发 生 器 来 生成 形式 的 矢量 点 集合 .注意 [1,1,1] e R' 与 
(+1, +1, +1,0,---,0] 所 有 可 能 的 排列 是 相同 的 ,其 中 有 n -3 个 0。 下 面 给 出 


一 些 实例 ,在 这 些 例子 中 ,使 用 符号 "” 
nb A n(n -1)--(n —k +1) (2 — 24) 
ny, À sata k tU (2 — 25) 
考虑 wu = 1 的 情况 -下面 例子 给 出 了 各 种 情况 下 生成 的 矢量 点 。 
例 2.1 对 于 [1] e R ,矢量 点 集合 由 矢量 组 成 
[1,0]' 
[1] e R A [0,1] (2 - 26) 


[-1,0]" 
[0,-1]" 
IFR 空间 , 当 n = 2i, EERIE ZERERA 2n = 4 个 矢量 点 ， 
如 图 2 -1 所 示 。 
例 2.2 对 于 [1] e R ,矢量 点 集 包含 
[1,0,0]" 
[0,1,0]" 
[I] e R° A 00,017 (2 -27) 
[-1,0,0]" 
[0,-1,0]' 
[0,0, -1]" 
这 里 ,矢量 点 落 到 三 维 超 球面 的 笛 卡 儿 坐标 轴 上 ,如 图 2 — 2 所 示 。 





图 2 -1 二 维 笛 卡 儿 网 格 坐标 轴 图 2-2 三 维 笛 卡 儿 网 格 坐 标 轴 
矢量 点 示例 矢量 点 示例 


512.3 对 于 [1,1] e R', 矢 量 点 集合 包含 2n2) = 2n(n -1) = 4 个 矢量 


HAr 
[1,1] e R A [1, -1]" (2 -28) 
[= 
[-1, -1]" 
例 2.4 对 于 [1,1] e RK’, MAHA 2n" = 2n(n - 1)(n -2) =2.3. 
2*1 =12 个 矢量 点 , 即 
[11.0] 
EF AGH 
[0,1,1]" 
[- 1,1,0]* 
[-1,0,1]' 
[1,1] e R^ A LO, = 1,1)" (2 -29) 
[1, - 1,0]" 
[1,0, - 1]! 
[0,1, - 1] 
[-1, - 1,0]* 
[-1,0, - 1]' 
[0, - 1, -1]’ 
通过 式 (2 - 24) 和 式 (2 - 25) 可 知 , 由 一 个 发 生 器 产生 的 矢量 点 的 数目 可 
以 由 下 式 ” 得 , 即 
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[0] e R" l 

[1] e R” 2n 

[1,1] e R" 2n (2 — 30) 
[iit] eX 2? psy 


[1,1,1,1] e R° 28 


23 多 参数 多 维 非 线 性 函数 估计 


对 于 许多 跟踪 应 用 ,估计 方法 需要 用 一 种 积分 值 进行 评价 ,这 种 积分 值 是 多 
维 参数 乘 以 一 个 概率 密度 得 的 非 线性 多 维 函 数 的 积分 。 本 节 将 回顾 数值 至 近 非 
线性 函数 方法 。 

本 节 首 先 分 析 标量 方法 ,然后 扩展 到 多 维 情况 .所 有 非 线性 函数 的 近似 实质 
上 都 是 将 非 线性 函数 扩展 为 一 个 具有 任意 系数 的 多 项 式 。 系 数 的 具体 取 值 取决 
于 实际 应 用 ,根据 精度 要 求 和 其 他 计算 考虑 因素 来 决定 。 估 计 和 跟踪 应 用 中 , 系 
数 的 取 值 规范 将 在 后 面 关 于 研究 特定 方法 进行 密度 加 权 积分 评价 的 章节 中 予以 
解决 。 

2.3. 1 节 回 顾 一 下 用 标量 化 参数 逼近 标量 非 线 性 函数 的 方法 .首先 讨论 标 
量 函 数 的 一 般 多 项 式 展 开 ,在 此 基础 上 推导 出 非 线性 函数 的 标量 泰勒 近似 多 项 
式 。 用 有 限 差 分 替代 泰勒 多 项 式 的 差 值 ,导出 泰勒 多 项 式 的 数值 逼近 ,得 Stirling 
插值 公式 (Stirling 多 项 式 ) 。 

接 下 来 的 小 节 中 ,通过 多 维 多 项 式 扩展 的 介绍 ,将 标量 估计 推广 为 具有 多 维 
参数 的 多 维 非 线 性 函数 的 估计 , 这 一 推导 再 次 得 广义 多 维 泰 勒 多 项 式 。 将 
Stirling 允 近 推广 至 多 个 维度 可 以 通过 多 种 方式 实现 ,其 中 ,每 一 种 都 会 引出 一 
个 不 同 的 数值 估计 方法 .要 导出 一 个 简单 的 多 维 Stirling 估计 ,需要 3 个 笛 卡 儿 
插值 点 ,其 中 n, 是 多 维 郴 数 参数 的 维度 。 下 面 简要 讨论 两 种 简化 方法 。 第 一 种 简 
化 方法 中 , 插值 点 可 以 减少 至 只 包含 那些 落 在 坐标 轴 上 的 点 , 数目 减少 至 
2n, + 1; 第 二 种 简化 方法 是 使 用 单 形 Stirling 逼近 ,将 插值 点 数目 减少 至 nm + 2。 

本 书 的 讨论 内 容 仅 限 于 估计 方法 ,几乎 不 会 涉及 同样 也 很 重要 的 通 近 误差 
估计 和 收敛 性 等 内 容 。 


2.3.1 标量 非 线性 函数 估计 


2.3.1.1 一般 多 项 式 展 开 


通常 来 说 ,任意 一 个 参数 为 标量 的 连续 非 线 性 标量 化 函数 A(x) ,都 可 以 用 
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任意 点 xm, 通过 一 系列 的 逼近 得 任意 精度 的 估计 , 即 
fG) = Yaa ~ x) = ay +a, (a - a) + 


a(x — z) ° + a,(x — x)? + (2 — 31) 
通常 很 难 找到 一 个 在 无 穷 范围 求 和 的 封闭 形式 的 解 ,因此 将 序列 截断 为 一 个 
M 阶 的 多 项 式 估计 , 即 


f(x) = 2, a, (z 7 x) = a +a,(x — xo) + 
a,(x — xo) ° ++ + ay(x — x)" (2 — 32) 
式 中 :a(x - xo) 这 个 多 项 式 的 每 一 项 都 可 以 进一步 展开 为 带 有 系数 b, 的 单项 
3X x 。 因 此 ,函数 所 zx) 的 多 项 式 估计 可 以 写成 一 个 展开 式 , 即 


M 
f(x) = Y bax = by + byx + bu +o + by x” (2 - 33) 
i=0 


by rw (2 -34) 
2.3.1.2 泰勒 多 项 式 展开 


下 面 给 出 M 阶 泰勒 多 项 式 和 麦克 劳 林 多 项 式 的 简单 推导 过 程 ,前 提 条 件 是 
f(x) B M 阶 导 数 存 在 。 令 式 (2 - 32) 中 的 x = xz, 得 


= [f(x) ]..., (2 -35) 
为 求解 a ,对 式 (2 - 32) 求解 关于 变量 x 的 导数 ,并 设 x = xo, 得 
= [dd 
= [e] (2 - 36) 


求 式 (2 732) 在 x = x 处 的 各 级 导数 ,对 于 每 种 情况 ,用 f(x) 的 高 阶 导 数 对 所 有 
系数 进行 评价 ,导出 著名 的 用 xo) 的 标量 化 泰勒 多 项 式 点 表 近 , 即 


f(x) = [f(x) ],- ,+ [Sy] I (x — xo) 


Za d go G 2 a M (2-37) 
- Xie x - =) [( jro) (2 - 38) 


通过 对 f(x) 的 运算 定义 标量 算 子 如 下 
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D. f(x) à (x - x9)'[ (2) £2) ] (2 - 39) 
M 阶 泰勒 多 项 式 (2 - 38) 变换 为 
Ka) = Y AD fix) (2 - 40) 


i=0 


有 些 情况 下 ,计算 f(x) AS Se BE TE PAE, 36 8) 4 1 CB a T YE AS AG 
过 三 项 (M = 2) 的 时 候 被 截断 ,在 Arfken 中 可 以 找到 一 个 略微 不 同 的 泰勒 多 
项 式 推导 ,得 出 由 于 截断 泰勒 多 项 式 产生 的 误差 ,M 阶 泰勒 多 项 式 的 误差 项 如 下 
(x 一 x9)" d\* 
Ry = ay (gg) E (2 - 41) 
式 中 :xo < £ < x. 
2.3.1.3 Stirling 多 项 式 展 开 


A — Rb f(x) 无 导数 通 近 方法 ,在 泰勒 多 项 式 副 近 中 利用 中 心 差分 法 来 代 
替 求 导数 ,通过 泰勒 级 数 展开 式 得 有 限 差分 允 近 。 假 设 函 数 (x*) 在 感 兴趣 区 域 
连续 可 微 , 并 且 f(xo) 及 其 在 x = x, 处 的 所 有 导数 已 知 。 根 据 式 (2 - 37) ,检验 
S(% + q) XF x, 的 一 阶 泰勒 多 项 式 


f(xo +q) = f(x) +f'(x0)g (2 - 42) 
q Š x — xo 
f'(x) A df( x,) /dx, 
按照 相同 的 步骤 ,得 f(x。-g) = f(x.) -f'(xo)q (2 - 43) 
将 两 个 等 式 相 减 , 得 一 阶 中 心 差 分 通 近 
f'G) = gcUG +9) -fx - 9] (2 - 44) 


这 里 用 Xi RF Xoo 
对 于 二 阶 泰勒 多 项 式 ,按照 同样 步骤 计算 ,得 


f(x +q) = fG) +f'(z)q + Sf" G0 (2 - 45) 


f(x - q) = f(x) -f'G)a + 4f" G0 (2 - 46) 


f'"(x) À d'f(x,)/dx 
将 上 面 两 个 等 式 相 加 ,得 


f(s) = afte +9) -2f(x) + f(x -q)] (2 - 47) 


AA FA Be BP 22 4) 38 Yr. 8 — HH XO - 37) 简化 为 二 阶 
Stirling ffi ffi Z3 , BD 


f(x) = flay) + DL fü) + Z D fG) (2 - 48) 
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D, fix) À 2 UC +q) -f(xo — q) (x — x0) (2 - 49) 


D lx) à cs Ufo) - 2/08) + fl% 7 1 =)? (2 -= 50) 


Mix, = 0 时 ,逼近 结果 显而易见 。 

上 面 就 是 对 f(x) 无 导数 允 近 的 方法 , 步 长 参数 g 可 取 任 意 值 ,对 上 面 二 阶 双 
近 算法 进行 检验 可 知 ,在 一 维 情况 下 ,f(x) YE xo xo + 9,xo - 9) 这 三 点 必须 已 
知 。 高 阶 副 近 可 以 通过 高 阶 导数 近似 来 简单 地 得 ,得 间距 为 q 的 一 维 均 匀 网 格 上 
评估 点 数目 的 增加 。 

为 总 结 标量 方法 ,本 书 给 出 了 函数 拟 x) 的 两 种 多 项 式 通 近 方 法 :泰勒 多 项 
式 扩展 和 Stirling 插值 公式 扩展 。 针 对 上 述 两 种 情况 ,本 书 检验 了 点 逼近 , 即 拟 x) 
在 点 处 的 多 项 式 逼 近 xo。 大 支撑 域 非 线性 函数 的 多 项 式 逼 近 则 是 一 个 更 广泛 的 
主题 ,超出 了 本 书 的 研究 范畴 。 


2.3.2 多维 非 线 性 函数 估计 


2.3.2.1 一 般 多 维 多 项 式 展开 


对 于 多 维 情况 , 可 以 将 非 线性 函数 写成 矢量 参数 f(x), HP Bf e RY 
Hx e R": 
f(x) = [f GO AlE) f GOD ]" (2 - 51) 
x o= [aistn] (2 - 52) 
注意 矢量 函数 可 能 和 x 维 数 不 同 .对 于 参数 为 矢量 的 标量 化 非 线 性 函数 ,一般 
的 关于 点 xo A [zol,xoz,…,xzow ] 的 d 阶 多 项 式 展开 可 以 写成 多 维 单项 式 的 形 
x , Hl] 
f(x) = f(x 27x) 
es M : a;(i, b. 8 ) ° 
Q0 i20 
(x, — xoa) (3; 7x95) (2n, 734,2" (2-53) 
其 中 i +i, +…+ 大 d 表 明 只 有 满足 特定 条 件 的 多 重 和 项 能 进行 多 项 式 展开 。 
从 下 例 中 可 以 清晰 地 看 到 这 一 点 。 
例 2.5 对 于 x e R ,由 d = 2 或 更 低 阶 次 基底 张 成 的 多 维 单项 式 由 一 组 6 
个 单项 式 构 成 11， (wr —%1), (x; — X932), (x, - X01 ) (x; = 493) s (X4 -Xoi at 
(x, = x93) | f(x) 关于 点 xo 的 多 项 式 通 近 如 下 所 示 
f(x) = f(x,,*,) = a(0,0) + 
a(1,0)(x, — x4,) *a(0,1)(x, — x45) + 


a(l,1) (x, — x91) (x; — X9.2) * 


a(2,0) (x, — xoa)? * a(0,2) (x, — x95)! (2 - 54) 
按照 式 (2 - 53) 的 形式 ,可 写成 


d d 


f(x) = > Y aC i) (x, - 191)" (x, - x02)” (2-55) 


i =0 ix =0 


其 中 多 项 式 中 的 每 一 个 单项 式 必须 满足 条 件 +i < d = 2. 
一 般 来 说 ,对 于 矢量 x e Rf (o $ “个 精度 为 4 或 者 更 少 的 单项 式 。 二 项 


式 系数 ”] 是 从 n 个 不 同 对 象 的 集合 中 选取 m 个 对 象 的 组 合 数 (不 考虑 顺序 ) , 
定义 为 
! 
网 à "Tr parti (2 - 56) 


XT A Hk PRÉC f(x) 关于 点 的 多 项 式 展 开 x, 可 写成 
f(x) = fix, ux) 
~ DD ai i, š.) 。 
ATO ip =0 
(xi — x94)" (%2 = woo) 2 (x, = xo...) " (2 - 57) 
E = [a,,2,,*,a,]* (2 - 58) 
5012.6 下面 再 次 考虑 x e R° 且 最 高 为 2 次 寡 的 情况 , 式 (2 - 57) 变换 为 
f(x) = a(0,0) *a(1,0)(x, x54) +a(0,1)(x — x95) + 
a(1,1) (x, — x91) (x4 — x95) + 
a(2,0) (x, - xo) * a(0,2) (x, — x95)? (2 - 59) 
Ji TR P] 15 PORE R Pe ARR AE pR BC BY) 28 39] e ji yC r [B] Bi ,然后 讨论 将 Stirling 插 
值 公式 向 多 维 推广 的 问题 。 


2.3.2.2 多 维 泰勒 多 项 式 展开 
在 x e R° Hf(x) e R" 的 情况 下 ,假定 式 (2 -59) 中 的 x = xo, 得 泰勒 多 


项 式 
a(0,0) = [f Cx) lean (2 - 60) 
然后 ,对 式 (2 - 59) 中 f(x) KF x, 取 偏 导数 , 令 x = x, 可 导出 
_fa 
a(1,0) = Lf] (2 - 61) 


9/àx, XE f(x) 中 每 个 元 素 执行 运算 。 
用 类 似 方法 ,由 式 (2 - 59) 确定 其 余 系 数 的 取 值 ,得 如 下 的 二 阶 多 项 式 展 
开 , 即 
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F(x) = FE) Jaor + [G 70 [29] : 
(x, - xoa) [s fO] ! + 


alc [Z0] | T 


205 = 34) (x2 = 83) | 9 — P] + 
Gs ca 24] 上 (2 - 62) 
EMULE BRT oS 9 
f(x) = Xi D, , f(x) (2 - 63) 


运算 符号 类 似 于 Williamson 等 NM ZA, 用 下 式 定 义 标量 运算 符 
Dix, Bl 


DL. JG) à [m6 ~ a) tO cma + ==" + 


x-G, -mw)] fo) |... (2 - 64) 


符号 [* ] sn 仅 适 用 于 微分 项 。 将 多 项 式 (x + … a) 展开 ,每 一 项 都 由 
xj x x 形式 的 因子 的 常数 倍 构成 ,其 中 非 负 整 数 i 满足 ii + +i, = i。 多 项 
式 展 开 形式 如 下 





(x, + +x,)' = yd, NL ac (2 - 65) 
x d i, 20 1 Lb, 
多 项 式 系数 可 由 下 式 求 得 , 即 
i i! 
ELE lab 
912.7 考虑 这 种 情况 
2 2 2 2 mE 
(x, x, + x) = 7. Y +. |， Jesi (2 - 67) 
i, =0 iy =0 ig =0 \ h 13 
iy tig tig =2 
= xi +x, +x; xx, + 2x,x, + 2xx, (2 — 68) 


式 (2 - 64) 可 以 直接 写成 
D, f(x) = E Èl- Je -= xoa) °° *(x,, = xon)" a 





mra ico W (2 - 69) 
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将 式 (2 - 63) FRA K EZ , RREA FP B3, 253] A LGB h 
果 为 一 个 多 维 函 数 
f(x) = CE) laun + [ V GO 1S — xe) + 


TARG -x,)'[ v. VG) Tu as Ge =x) (2-70) 
i p= 


一 阶 项 是 雅 可 比 矩 阵 [ VS (x) ]7.,, e RY, 二 阶 项 是 一 个 Hessian 矩阵 ， 
第 p 个 分 量 为 [ V, VES) laon e R”, e, 是 一 个 沿 p 维 笛 卡 儿 坐标 轴 方向 的 
n, 维 单位 矢量 。 很 难 计算 多 维 函 数 的 超出 一 阶 项 的 泰勒 多 项 式 展开 ,所 以 通常 
截断 多 项 式 序列 ,使 其 没有 高 阶 项 。 


2.3.2.3 BH Stirling 多 项 式 展开 
SABER OM Siding imasta RUIN 
fix) ~ Y ADL (2-71) 
或 者 仅 保留 至 二 阶 为 
f(x) = fos) + Daaf) ea Diu Go (2 -72) 


中 心 差分 算 子 DLL f(x) 和 D2, f(x) 定义 为 
D, f(x) = LT (x, = Xo.) Lf Xo + qe,) - f(x, — ge, ) | (2 - 73) 


BD? fx) & GLY (z, - top fs + qe,) -Ax0) +fGa -q6.)] + 


ix > (x, — %,,)(%, — x9,) * 


[f(xo + qe, + ge,) -f(x - qe, + qe,) - 
f(xo + qe, - qe,) + f(x, - qe, - qe,) ] | (2 - 74) 
式 中 :e, Me, 分 别 为 沿 着 第 p 维和 第 ~ 维 笛 卡 儿 坐标 轴 的 单位 矢量 。 
例 2.8 对 于 x e R’ ,得 


D, ,f(x) à zi (x, = x D [f(xo + qe) - f(x, - qe,)] + 
(x; — xo 5) [f(xo + qe;) - f(x, - ge,) J} (2 - 75) 


D: f(x) À "is - x5)? [f(xo + qe) - 2f(xo) +f(xo - qe, ) ] + 


"i - xui) [f(xo + qe) -2f(x,) +f(xo - qe,) ] + 
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Z (x = x94) (x; -= 392) ° 
q 


[f(xo + ge, + qe,) — f(x, — qe, + qe,) - 

f(x, + qe, - qez) * f(x, - qe, - qez) | (2 - 76) 
显然 ,插值 x e R 需要 一 组 均匀 分 布 在 二 维 网 格 上 的 9 个 间距 为 gq 的 点 ,Je 
| (Xo, »¥0,2) (xoa £q,xo2),(Xoa 262 +q) , (X01 土 9,X0.2 土 9)| ,如 图 2 -3 所 示 。 

通常 ,二 阶 多 维 Stirling 多 项 式 通 近 所 需 点 的 个 数 为 3”*, 在 均匀 多 维 网 格 
(立方 体 ) 上 ,间距 为 9。 


(x, 74.351 +q) (Ss *q) (x, +a, Xo +q) 


(x: 74.39; (5; +q,x.:) 





(x, 74.3, -4) (x; -4) (so +q.x,; -q) 
图 2-3 多 维 Sterling 逼近 的 二 维 点 选取 


另外 一 种 方法 利用 Hermite 正 交 多 项 式 5? 函数 展开 式 来 代替 等 
式 (2 - 63) 和 式 (2 - 69) ,得 相同 数目 的 插值 点 作为 二 阶 多 维 Stirling 3 yr (ih 
值 点 可 能 并 不 相同 ) ,这 种 方法 不 需要 进行 求 导 或 者 进行 有 限 差分 运算 ,在 很 多 
方面 优 于 有 限 差 分 方法 .当然 ,以 上 陈述 纯 属 个 人 的 主观 意见 。 这 种 插值 法 将 在 
后 续 章 节 加 以 介绍 。 

之 所 以 把 Hermite 插值 内 容 介 绍 放 在 后 面 ,是 因为 可 以 通过 一 个 简单 的 变 
换 , 把 高 斯 加 权 积分 转化 成 Hermite 积分 ,使 算法 更 易于 理解 。 

还 有 其 他 的 无 导数 插值 方法 , 比 上 述 多 维 Stirling 多 项 式 逼 近 方 法 需要 更 少 
的 插值 点 。 这 种 插值 方法 只 需要 用 到 图 2 - 3 中 那些 落 在 正 交 笛 卡 儿 坐 标 轴 以 及 
原点 上 的 插值 点 。 对 比 式 (2 - 73) 和 式 (2 -74) ,可 发 现 式 (2 -74) 中 只 有 交叉 
项 被 去 掉 了 。 对 于 二 阶 项 来 说 ,精度 损失 的 代价 非常 小 ,经 过 这 一 简化 ,插值 点 的 
数目 减少 至 2n, + 1。 

男 一 种 多 维 插值 方法 利用 原点 上 的 点 和 男 外 的 多 维 超 球面 上 等 角 间 距 点 进 
行 插值 ,这 种 方法 称 为 球面 单 形 插 值 .图 2 -4 给 出 一 个 顶点 在 超 球 面 上 的 三 维 
正则 单 形 图 (所 有 边 和 角 都 相等 ) 的 例子 ,这 些 方法 仅 需 要 n, + 2 个 点 即 可 实现 
二 阶 捅 值 . 虽 然 对 于 多 维 函 数 来 说 , 单 形 插值 方法 是 数值 积分 方法 的 基础 ,但 直 
到 最 近 ,采用 单 形 方 法 进行 多 维 插值 才 见 诸 文献 [10 - 16] ,具体 为 计算 机 图 形 
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学 相关 应 用 。 因 此 , 单 形 插 值 方法 的 详细 介绍 超出 了 本 书 的 范围 ,尽管 如 此 ,本 书 
将 在 第 10 章 给 出 用 单 形 法 进行 多 维 函 数 的 高 斯 加 权 积 分 。 





2 维 3 维 
1 维 | | 
线 
三 角形 
四 面体 


图 2 -4 1-3 维 单 形 图 
2.4 多 变量 统计 概述 


2.4.1 一 般 定 义 


从 贝 叶 斯 观点 来 看 ,随机 变量 是 指 变量 的 值 无 法 预先 确定 ,可 以 根据 概率 分 
布 改变 的 量 。 随 机 变量 通常 是 多 维 的 ,所 以 其 概率 分 布 是 多 元 的 .此 外 ,随机 变量 
的 期 望 值 可 以 随时 间 变化 ,分 布 随时 间 的 变化 由 动态 方程 或 转移 方程 决定 。 


2.4.1.1 概率 密度 函数 
这 一 节 首 先 讨论 概率 密度 函数 和 多 维 随机 变量 的 累积 分 布 函数 的 一 般 


定义 。 
多 维 随机 变量 x 在 点 x = £ 处 的 概率 密度 函数 可 定义 为 "” 
p,(£)d£ À Prob|£ < x <£ +d£#| à Pl£ < x <£ + dë! > 0 
(2 - 77) 
Jeh d£ 是 一 个 无 限 小 区 间 。 得 出 x HUE IX IR] m < x < 内 的 概率 为 


Problm < x sé} AP\lyn<x<€! -f p, Cx) dx (2 - 78) 
其 中 心 为 x 的 维度 。 多 维 积分 定义 如 下 f 
f p G0dx a fax fn, p Qn enn 


= [Tf axo. (ps n) (2 - 79) 
iz] ~ mi 


x HEKE ny < x < 的 概率 为 区 间 中 概率 密度 函数 的 多 维 “ 体 积 ” ,概率 密度 也 
数 更 常用 的 表达 式 是 用 p(x) 来 表示 p. (x) 。 


2.4.1.2 累积 分 布 函 数 
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P, {é| A Problx < £| -f p,(x) dx (2 - 80) 
EEE o 时 ,由 于 概率 密度 函数 通常 规范 化 为 1,P, lE) 趋 近 于 1 .概率 密度 
函数 和 累积 分 布 函数 之 间 关系 如 下 
_fd 
p(x) = [eie] (2-81) 


£-x 
2.4.1.3 联合 概率 密度 函数 和 边界 概率 密度 函数 


两 个 多 维 随机 变量 的 联合 概率 密度 函数 是 联合 事件 出 现 的 概率 p(x,z) ox 
的 边界 概率 密度 函数 定义 为 


p(x) = | p(x,z)d (2 - 82) 

条 件 z 下 x 的 条 件 概率 分 布 是 
p(x z) _ Pun (2.= 88) 

p(z) 

因此 ,x 的 概率 分 布 函数 可 以 由 下 式 得 , 即 

p(x) = [plxsz)dz = | Prlz)p(z)dz (2 - 84) 
2.4.1.4 贝 叶 斯 法 则 
贝 叶 斯 法 则 服从 条 件 概率 
p(x,z) = p(x| z)p(z) = p(zl x)p(x) (2 - 85) 
delde p(z! cu: _ B: x)p(x) (2 ~ 86) 
pus | pe x)p(x)dx 


由 此 ,有 如 下 比例 ( oc 表示 比例 ) , 即 
p(x! z) < p(z! x)p(x) (2 - 87) 


2.4.1.5 PRE ES AY fit ( E) 


假定 随机 矢量 x A [x ,x,,…,x, ] e R 的 概率 密度 函数 为 p(x) ox 的 期 
望 值 (一 阶 矩 ) 定义 为 
£&ezix| = | x,(x)dx (2 - 88) 
协 方差 (二 阶 矩 ) 为 M W 
P“ A e|(x -X)(x -x)"} 


= Í (G -h(x - š)'p(x)dx 
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= | xx'p(x)dx - 27 (2 - 89) 


对 于 两 个 n, 维 随机 矢量 x À [xi ,x,,… ,x ] Mz & [z,,2,77,z,] ,两 个 变量 之 
间 的 互 协 方差 定义 为 
P“ A s|(x -x)(z-z)!| 


= ` (x - ê) (z - š) 'p(x z) dxdz 


= 三 xzp(z,z)drdz - X2! (2 - 90) 
注意 ,一 些 概率 密度 的 矩 是 不 存在 的 ,这 是 因为 这 些 矩 的 积分 是 无 限 大 的 。 


2.4.2 ”高 斯 密度 


1733 4£ , AbrahamdeMoivre ^ 在 一 篇 论文 中 首次 介绍 了 高 斯 概率 密度 函 
此 , 自 此 其 在 估计 理论 中 的 地 位 就 非常 重要 。 因 为 这 种 对 称 分 布 具 有 简单 的 解析 
形式 ,对 于 标量 和 矢量 随机 变量 都 很 适用 ,可 以 在 许多 领域 中 用 于 噪声 过 程 建 
模 。 在 线 参 考 文献 [19] 中 总 结 了 高 斯 分 布 的 属性 ,下 面 将 讨论 高 斯 密度 的 特性 ， 
这 对 本 书 提 出 的 递归 估计 方法 非常 重要 。 首 先 介绍 高 斯 概率 密度 函数 ,然后 给 出 
高 斯 累积 分 布 函 数 的 推导 ,之 后 介绍 一 种 仿 射 变换 ,将 坐标 系统 的 原点 变换 至 高 
斯 密度 的 均值 ,旋转 坐标 轴 使 其 与 密度 函数 的 主轴 重合 。 这 样 做 主要 是 为 了 简化 
第 IL 部 分 中 提出 的 高 斯 加 权 积分 方法 。 最 后 ,给 出 变换 矩 方程 一 组 恒等式 。 


2.4.2.1 高 斯 概率 密度 函数 


在 估计 随机 变量 的 矩 的 过 程 中 ,最 常用 的 假设 是 概率 分 布 符合 高 斯 分 布 。 一 
维 随机 变量 x 服从 高 斯 概率 密度 函数 ,如 果 
p(x) = N(x;x,0") A === - goals - 21°} (2 -91) 
式 中 :x 为 一 阶 矩 或 者 均值 ;c” 为 二 阶 矩 或 者 方差 (er 通常 称 作 标准 差 ) .图 2 -5 
中 给 出 了 关于 x 的 一 维 高 斯 概率 密度 函数 的 示例 ,其 中 x = 5.0 = 6. 
n, 维 随 机 变量 x 的 高 斯 概率 密度 函数 定义 为 
p(x) = N(x;z,P") À (27)™" | P” I "72. 


exp{- [x - #]"(P*) [x - 81] (2 - 92) 


SRP xm P* 分 别 为 矢量 均值 和 协 方差 矩阵 ;1 4 1 为 矩阵 4 的 行列 式 。 注 意 p të 
. 终 为 一 个 标量 。 
假定 二 维 高 斯 概率 密度 郴 数 
£= [xy] 2[-5,2]' (2 - 93) 
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-30 -20 -10 0 10 20 30 
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图 2 -5 一 维 高 斯 概率 密度 函数 示例 
Ho, =6,0, 28,9 =0.5, 将 产 — P” ,得 


ue o. po, [60 24 
P= ka c, | N " 
二 维 互 协 方差 可 写成 如 下 形式 
e 5 
EVA ir 7| 


2 
Uw Š poo, 


图 2 - 6 给 出 了 这 种 概率 密度 函数 的 图 形 化 描述 。 





-40 一 40 x 


2-6 二 维 高 斯 概率 密度 函数 示例 


2.4.2.2 高 斯 累积 分 布 函数 


多 维 高 斯 累积 分 布 函 数 (cdf) 的 定义 为 
P(x) Af p(u)du = [f N(u;x,P™) du 
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(2 - 94) 


(2 - 95) 


= (2m) "2 p= | 1⁄2 , 
| exp{- T [u -£]'(P#)'[u -£])du (2 - 96) 


4 
t(u) A D'(u-x) (2 -97) 
D 可 以 定义 为 
P” = DD' 
则 P(x) 变 为 
P(x) = it = am ea (2 - 98) 


式 中 :4 为 上 的 第 7 个 元 素 。 括 号 内 的 一 维 方程 可 重 写 为 


l A t</2 1 -Ü 
mi e i dt (u) 一 >= m. dt + 
l j -2⁄2 
— c— j d 
v = A. t; 
l 1 2 j -72 
3 d a) (2 -99) 
注意 MATLAB? 中 的 函数 定义 为 
aiy] x MEE (2 ~ 100) 
此 时 多 维 高 斯 累积 分 布 函数 变 成 
P(x) = Hu + erf[t;] | (2 - 101) 


图 2 -7 W2 -8 分 别 给 出 一 维 、 一 维 累积 分 布 函数 的 例子 ， 其 中 均值 和 方差 的 取 
值 和 上 面 一 维 、 二 维 概率 密度 函数 示例 相同 。 





3⁄ 0.6 
Ë 05 
0.4 
ni 0.3 
0.2 
0.1 
30 20 10 0 10 20 30 
图 2 -7 ”二 维 高 斯 概率 密度 函数 示例 图 2 -8 二 维 高 斯 累积 分 


布 函 数 的 例子 
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2.4.2.3 一 般 高 斯 矩 方程 的 变换 


注意 矩 方程 (2 - 88) 和 方程 (2 - 89) 中 ,积分 的 形式 为 xp(x)dx。 
对 于 一 维 高 斯 情况 ,考虑 更 通用 的 矩 积分 为 
£|/G) 1 = ff) Gs, )ds 


= | /9 (en{- ts -1 (2 - 102) 
进行 一 维 仿 射 变换 
x = X + oc (2 - 103) 
该 变换 将 坐标 原点 x 进行 了 平移 并 且 对 标准 差 进行 了 比例 变换 , 故 N(x;x ,0 ) 一 
AM(c;0,1)/o。 其 中 ,c 是 一 个 均值 为 零 的 单位 标准 差 随机 变量 。 


$ 


f (c) À f(Š + ac) (2 - 104) 
H dx — odc ,一 般 的 和 矩 积分 方程 (2 - 102) 可 变 为 
EIS} = | f(e)M(e;0,1)de (2 - 105) 


类 似 地 ,对 于 多 维 情况 ,考虑 一 般 矩 积分 
Elf(x)| = | foo NGsk, P") dr 


= | foo? 1 pri. 


exp{ -3[x - £] CP)" [x - š] fax (2 - 106) 
现在 进行 多 维 仿 射 变换 


x = x + Dc (2 - 107) 

P” = DD' (2 - 108) 
得 恒等式 

D = [ P#]'2 (2 - 109) 


这 里 ,DD 可 以 取 P* 的 Cholesky 因子 或 P* 主 (正定 ) 矩阵 的 根 ,实际 上 ,任何 矩阵 
D 只 要 满足 式 (2 - 108) 中 的 定义 就 足够 了 ,此 外 ,ec 可 以 看 成 是 具有 单位 对 角 协 
方差 矩阵 的 零 均值 多 维 随机 变量 , 即 c ~ N(€;0,1). 
式 (2 - 107) 中 的 仿 射 变换 有 一 个 几何 解释 。 多 维 高 斯 分 布 的 等 概率 轮廓 的 
是 以 均值 为 中 心 的 椭 球 体 。 主 轴 的 方向 由 协 方差 矩阵 的 特征 矢量 决定 ,特征 值 等 
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于 主轴 相对 长 度 的 平方 。 式 (2 - 107) 所 示 变 换 将 坐标 原点 平移 至 x 坐标 ,并 将 
坐标 轴 旋 转 至 与 密度 函数 的 主轴 重合 ,同时 将 每 个 轴 比 例 变换 至 标准 差 为 1 ,如 
图 2 -9 所 示 。 注 意 ,转换 后 的 密度 的 轮廓 是 圆 形 而 不 是 椭圆 形 。 
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"x x 


图 2 -9 对 高 斯 概率 密度 函数 进行 仿 射 变换 的 效果 





考虑 到 
[. f(x) N(x;$,P™)dx = B (2 - 110) 
将 式 (2 - 110) 和 式 (2 - 107) 代入 式 (2 - 106) ,得 
Eifix)| =[ F (e) Nes0,Dde (2 - 111) 
f (c) À f(X * Dc) (2 - 112) 


这 种 仿 射 变换 的 应 用 是 一 个 非常 重要 的 概念 ,可 将 其 用 于 简化 高 斯 加 权 积 
分 的 数值 估计 方法 , 建立 贝 叶 斯 估计 的 矩 方程 组 。 使 用 这 种 仿 射 变换 , 结合 第 
2.3 节 中 讨论 的 非 线性 函数 的 多 项 式 展开 ,简化 了 这 些 积分 的 数值 估计 过 程 。 


2.4.2.4 高 斯 矩 方程 
本 节 将 介绍 (并 不 推导 ) 一 些 适用 于 高 斯 矩 方程 的 恒等式 。 


对 于 一 个 随机 变量 x e R 和 一 个 对 称 矩 阵 A e R" ,标量 化 数量 x Ax 被 
PON x 的 二 次 型 ,可 以 表示 为 
Ejx'Ax} = tr| AP} +X'Ax (2 = 113) 


式 中 :x 和 尸 分 别 是 x 的 期 望 值 和 协 方差 矩阵 ;| 上 } 表示 括号 内 矩阵 的 迹 。 这 一 结 
果 仅 需要 x 和 书 存 在 ,而 与 概率 密度 函数 的 形式 无 关 -. 用 x 的 四 次 形式 表达 期 望 
值 如 下 5 

Elx Axx A,x} = trl4Pitrl4P +2tril4,P4)PI (2-114) 
在 一 维 情况 下 ,高 斯 矩 方程 包含 积分 项 | e M(e;0,1)de i Fê = 0, 并 且 Ntc; 
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0,1) 关于 0 对 称 ,得 的 矩 方程 如 下 
1 


Eici = f c"N(c;0,1)de = f x3x5--(n-1), n 为 奇数 


0, 


n 为 偶数 


(2 - 115) 


根据 式 (2 - 115) 的 一 维 矩 方程 ,得 ec = [6.0.7.6] 中 分 量 的 四 阶 矩 方 


程 如 下 


[Me;0,Dde = [Ne 0,1) de, f^ NC, 30,1) de, Ed 


[c-NCGes0 D de = Je.W(e,30,1) de | Me: ;0,I)dc" = 0 


[ci (e; 01) de = 8; 
feseje.N(e30,1) de = ó [e.N(c, 30,1) de, š 
Sx |o;M(c 30,1) de; + 
a [cM Ce; 0,1) de, 
= 0 
[cic Nes.) de = 加 i 


L = 


Jee e NC; 0,7)dc = Ou + Ou + Ou + 36 iu 


T 
è” Á [c,,"“ 504 Cio Cy, ] 


为 计算 方便 ,去 掉 所 有 积分 的 隐 式 积分 极限 ,得 
5. A P ix Jj 


g = 


l, i=j 
a.a P: i=j=k=l 
0, 其 他 
afl, i=jk=lhij=kl 

0, 其 他 


6,6, 75,6, 的 定义 与 上 面 类 似 。 
由 式 (2 - 113) 和 式 (2 - 114) ,18 e 的 二 次 矩 方程 如 下 
1, i=0 
[Le'MeY Ne; 0,1)de = ran S, Red 
[tr MI ]? +2tr| MM}, 
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(2 - 116) 
(2 - 117) 
(2 - 118) 
(2 - 119) 
(2 - 120) 
(2 = 121} 
(2 - 122) 
(2 - 123) 
(2 — 124) 
(2 -125) 


式 中 :M e R" 为 一 已 知 对 称 方 阵 。 
根据 式 (2 - 116) 至 式 (2 - 121) ,很 容易 证 明 


fec" tes 0,D)dc = I (2 - 126) 


2.4.2.5 使 用 MATLAB8 从 多 维 高 斯 分 布 中 生成 采样 


一 般 多 维 高 斯 分 布 N(x;x,P™) 中 的 采样 可 以 在 MATLAB 中 采用 如 下 方式 
生成 : 

(1) ERREA cO ~ N(ce;0,D) ,i = 1,…,n,| 。 这 里 ,符号 ~ 代表 从 密度 
中 选取 的 第 i 个 采样 NM(c;0,7) YE MATLAB 中 ,使 用 命令 c = randn( N,M) KË 
成 采样 ,其 中 N 为 ¢ 的 维度 ,M 是 所 需 的 采样 个 数 。 

(2) 使 用 式 (2 - 107) 将 对 e 的 采样 变换 为 对 x 的 采样 ,得 

x) = x+ Det? (2 - 127) 

st'}:D HA P” = DD'. 
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第 3 š 
贝 叶 斯 估计 的 基本 概念 





估计 过 程 通过 实验 得 一 组 观测 样本 ,通常 是 某 种 形式 的 数据 。 观 测 数据 可 以 
是 对 某 个 特定 目标 的 方位 和 (或 ) 范围 的 连续 时 间 采 样 ,或 者 是 一 个 交易 日 内 股 
票 价格 的 连续 采样 。 根 据 观 测 数据 ,可 以 对 一 些 无 法 直接 观测 到 的 特征 参数 进行 
估计 。 例 如 ,通过 方位 观测 ,可 以 用 时 间 相 关 的 函数 估计 出 目标 位 置 和 速度 ;而 对 
于 股票 价格 数据 , 则 可 以 估计 出 股票 的 波动 。 

本 书 集中 讨论 了 多 种 估计 方法 ,包括 随时 间 动 态 变化 的 变量 的 模型 估计 、 可 
观测 数据 和 不 可 观测 变量 之 间 关 系 的 模型 估计 ,特别 指出 ,本 书 的 讨论 将 局 限于 
离散 采样 数据 的 递 推 估计 方法 ,因此 , 书 中 假定 待 估计 参数 服从 已 知 的 递 推动 态 
过 程 ,而 且 观 测 数 据 和 估计 参数 值 之 间 的 解析 关系 已 知 。 

此 外 , 贝 叶 斯 估计 假设 待 估计 参数 和 观测 数据 都 是 随机 的 。 观 测 数据 和 待 估 
计 参 数 之 间 的 解析 关系 (一 种 变换 ) 为 估计 提供 了 一 个 统一 的 框架 ,其 中 递归 推 
理 以 当前 和 以 前 所 有 观测 作为 已 知 条 件 ,当前 状态 矢量 的 密度 函数 来 表示 。 


3.1 贝 叶 斯 估计 


贝 叶 斯 估计 的 目标 是 ,给 定 一 个 观测 矢量 z, 估 计 参 数 矢 量 x 的 连续 值 , 一 般 
YE x 和 z 作为 随机 矢量 来 处 理 。 对 于 参数 矢量 ,控制 参数 动态 变化 的 方程 本 身 就 
有 随机 假设 ,其 中 添加 未 建 模 影响 因子 作为 随机 噪声 ,假设 随机 测量 噪声 存在 ， 
可 以 证 明 观 测 矢量 的 随机 性 质 。 假 设 随机 矢量 x 有 已 知 的 先 验 密 度 函 数 p(x)， 
先 验 分 布 包括 在 得 任何 可 观测 数据 之 前 的 已 知 和 未 知 的 所 有 参数 矢量 。 如 果 参 
数 x 的 真 值 已 知 ,那么 z 的 概率 密度 可 由 条 件 密度 或 似 然 肾 数 p(z1x) 表示 ,z 的 
完整 统计 特性 已 知 。 

曾经 进行 过 这 样 一 项 试验 ,随机 变量 z 已 知 ,可 通过 贝 叶 斯 法 则 得 x 的 后 验 
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条 件 密 度 为 
gis) = PSS! 21000) (3 - 1) 
p(z) 
因此 ,在 贝 叶 斯 框架 下 ,后 验 密度 p(x | z) 中 包括 考虑 试验 观测 结果 之 后 ,需要 
了 解 的 zx 的 所 有 信息 。 由 于 试验 结果 z 已 知 , 式 (3 - 1) 中 的 分 母 就 是 一 个 归 一 化 


标量 常数 ,可 以 得 
p(z) = [p(z1 x)p(x)dx (3 -2) 
R^ 


对 于 完全 贝 叶 斯 估计 问题 的 似 然 密度 和 后 验 密度 ,或 者 它们 的 联合 密度 , 定 
义 估计 的 统计 模型 ,其 中 参数 的 联合 密度 和 观测 矢量 定义 为 
p(x,z) = p(xl z)p(z) (3 -3) 
基于 后 验 分 布 估计 问题 的 解决 方案 如 式 (3 - 1) ,因此 后 验 分 布 可 以 按 需要 
生成 x 的 点 估计 (如 果 存 在 点 估计 ) .注意 ,后 验 密 度 被 认为 是 估计 问题 最 一 般 的 
解决 方案 ,在 很 多 情况 下 可 以 用 密度 函数 来 描述 x. 


3.2 点 估计 式 


后 验 密 度 的 评价 是 一 个 非常 困难 的 问题 ,是 多 维 推理 问题 的 复杂 解 。 点 估计 

x 是 在 给 定 观 测 值 情况 下 ,对 参数 值 进 行 有 根据 的 推测 .基于 所 有 得 观测 值 , 生 

成 点 估计 的 解析 方法 称 为 x 的 估计 器 。 使 用 的 估计 器 不 同 ,得 的 zx 的 实际 值 不 

同 。 即 便 使 用 同样 的 估计 器 ,每 次 试验 得 的 值 也 不 同 。 因 此 ,点 估计 本 身 被 当做 一 

个 随机 变量 。 

为 找到 合适 的 估计 器 ,定义 一 个 代价 函数 L(x,x) ,对 于 错误 的 估计 x 关 x 进 

行 惩罚 ,通常 , 代 价 函数 的 惩罚 是 根据 误差 的 大 小 x -x 确定 的 ,在 不 失 一 般 性 的 

情况 下 ,假设 代价 函数 为 正 ,L(x,x) = 0 是 其 唯一 最 小 值 , 对 于 点 估计 x, 贝 叶 斯 
风险 R 为 代价 函数 的 期 望 值 

R À EIL(x,x)| (3 -4) 

根据 x 和 z 求 得 期 望 值 ,注意 ,使 用 观测 值 z 来 生成 x, 可 写 为 x(z) .xz(z) 的 最 优 
解 是 使 贝 叶 斯 风险 最 小 的 值 

x(z) = arg min | L(x' (z) ,x)p(x,z) dxdz (3 - 5) 


Arty 


这 里 的 最 小 值 是 通过 所 有 的 x"(z) 值 求 得 ,z 和 x 的 维 数 分 别 是 n, A n,o 
将 式 (3 - 3) 代入 式 (3 -4) ,把 积分 项 分 成 两 部 分 , 即 


&(z) = arg min | | [L(x"(z),x)p(xl z)dx|p(z)dz (3 - 6) 
R^; R" 


HT LO ,x) 和 p(x1z) 都 是 正 数 ,给 定 任何 z 值 ,内 积分 均 为 正 。 对 于 每 一 个 z， 
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AA p(z) 也 是 正 数 ,使 风险 最 小 的 x* (z) 值 也 使 内 积分 最 小 , 即 
f(z) = arg mh | Lt (z) ,x) p(x! z)dx (3 -7) 
式 (3 -7) 定义 的 贝 叶 斯 估计 问题 中 ffr PROBUS [0] ,产生 的 估计 器 也 各 不 相同 。 


还 有 另外 一 种 方法 一 一 最 大 代价 最 小 化 (最 小 最 大 估计 器 ) 可 选 作 估计 
ar ,例如 


x(z) = arg min maxi L(x" (z) ,x) | (3-8) 
对 于 大 多 数 估计 间 题 ,假定 代价 是 基于 下 面 的 估计 误差 
e À x - X (3 -9) 


则 代价 函数 L( e") 是 一 个 单 变量 函数 。 
(1) 一 种 有 代表 性 的 代价 函数 是 加 权 平 方 误差 代价 函数 
L(e) =e Me (3 - 10) 
它 强调 了 较 大 误差 的 影响 。 这 里 ,M 是 已 知 的 平方 加 权 和 矩阵 ,不 依赖 于 x。 
(2) 使 用 绝对 误差 值 代价 函数 
Ze) =l £| (3 -11) 
(3) 代价 函数 在 一 些 区 间 取 值 相 等 而 在 其 余 区 间 取 值 为 零 。 例如, 设置 
Lie) 如 下 
ie = 位 le! <A? (3 - 12) 
l, I €! > A⁄2 
具体 代价 函数 的 选择 取决 于 估算 方法 要 实现 的 结果 .通常 ,很 难为 一 个 主观 
的 量 指定 一 个 解析 测度 ,如 上 所 述 ,选择 代价 函数 的 目标 是 要 找到 一 个 估计 值 ， 
使 代价 的 期 望 值 最 小 。 
考虑 最 小 化 加 权 平 方 误差 代价 函数 的 期 望 值 ,通常 称 为 均 方 误差 准则 ,在 
式 (3 -9) 中 ,用 zx” 替换 x, 将 结果 代入 式 (3 - 10) , 则 式 (3 -7) 可 变 为 


F(z) = arg min | (x - x')'M(x - x" )p(xlz)dr (3-13) 
R"x 
为 得 参数 的 最 小 值 ,关于 zx” 求 导 , 将 结果 设 成 0, 求 解 x” , 则 可 以 得 


T 
as - x°" )'M(x - x* )p(x! z)dx 


=- 2M | spfx! z)dx + 2Mx^ [pce z)dx (3 - 14) 
Rn, 
将 它 置 为 0， 并 将 第 二 个 积分 项 置 为 1 得 
x. = [xp(xl 2)dx à £lx z] (3 - 15) 
因此 , 均 方 误差 代价 函数 最 小 化 的 结果 是 与 后 验 密度 (或 条 件 均值 ) 相关 的 平 


均值 。 
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由 于 均 方 估计 器 得 的 估计 是 随机 变量 ,要 量化 这 些 估 计 的 不 确定 性 ,一 种 简 
便 的 不 确定 性 测度 由 条 件 协 方差 矩阵 表示 , 即 
P*-£|(x-x)(x-x)'l zl 


s [o - X)(x -X)'p(xl z)dx (3 - 16) 


必须 注意 的 是 ,对 于 一 些 概率 密度 函数 ,不 存在 一 个 或 者 更 多 的 点 估计 。 柯 
西 分 布 是 一 种 没有 均值 . 协 方差 或 高 阶 矩 的 分 布 。 然 而 , 它 的 众 数 和 中 位 数 都 有 
明确 定义 , 协 方差 可 以 用 离 差 度量 来 代替 。 

本 书 致力 于 利用 平均 平方 代价 函数 的 估计 方法 。 绝 对 误差 和 统一 的 代价 函 
数 可 以 导出 最 大 后 验 ( MAP) 估计 器 最 大 似 然 估计 则 是 ,MAP 估计 的 一 个 特例 。 
这 些 估计 器 的 完整 推导 和 讨论 可 参阅 文献 [1] 中 第 56 - 58 页 。 


3.3 ”基于 概率 密度 函数 的 递 推 贝 叶 斯 滤波 器 


离散 动态 过 程 是 系统 的 当前 状态 依赖 于 之 前 一 个 或 多 个 状态 的 过 程 ,在 连 
续 过 程 中 ,当前 状态 对 前 一 个 状态 的 依赖 可 以 用 微分 方程 来 表示 。 对 于 离散 时 间 
观测 ,只 能 在 离散 时 间 上 进行 以 观测 为 条 件 的 估计 ,微分 方程 可 以 用 与 之 相等 的 
有 限 差分 代替 ,并 将 观测 时 间 为 t, 时 的 状态 和 在 i 之 前 的 观测 状态 联系 起 来 。 
一 阶 马尔 可 夫 过 程 中 当前 状态 只 取决 于 上 一 个 状态 ,因此 ,可 将 离散 随机 马 
尔 可 夫 动 态 过 程 表示 为 
z, 2 faal Eaa] = Salna E tP (3 — 17) 
式 中 :x, Wt, BF2|09 RES K2S f...) ,为 确定 性 转移 函数 (和 矩阵 ) ,实现 从 14,.， 
AY AUB ¢, 时 刻 状态 x 的 改变 ;u, 为 已 知 的 (通常 是 确定 性 的 ) 控制 ,构成 驱动 动 
态 系统 的 外 部 输入 。 
尽管 白 噪声 wn( 不 一 定 是 高 斯 ) 可 以 在 输入 时 开始 出 现 ,并 通过 转移 咀 数 变 
换 , 但 通常 假定 噪声 是 加 性 的 ,用 来 表示 真实 转移 函数 中 未 建 模 的 部 分 .注意 上 
面 给 出 的 马尔 可 夫 过 程 只 是 真实 转移 函数 的 模拟 。 例 如 ,如 果 将 一 盘 移 动 的 船舶 
的 运动 建 模 为 匀速 运动 ,噪声 项 代表 波浪 作用 导致 直线 路 径 微小 偏离 的 加 速度 。 
关注 的 具体 问题 是 对 通常 不 可 观测 的 状态 矢量 x, 的 估计 基于 所 有 试验 观 
WREE Zin A |z ,zs，…,z,| 的 集合 。 假 定 所 时刻 的 观测 矢量 和 i, 时 刻 的 状态 矢 
量 之 间 的 解析 关系 已 知 ,表示 为 
z, = h,(x, ft,) = h,(x,) + w, (3 - 18) 
式 中 :z, 为 观测 矢量 ;hh, 为 确定 性 观测 函数 ,给 出 状态 矢量 和 观测 矢量 的 关系 ,日 
噪声 jx, 不 一 定 是 高 斯 噪声 ) 可 以 通过 转移 函数 变换 ,但 通常 假定 观测 品 声 是 加 
性 的 。 
对 于 多 数 感 兴趣 的 问题 , 几 ,， Al h, 是 缓 变 的 ,随时 间 的 变化 非常 缓慢 ,因此 ， 
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可 以 认为 它们 是 与 时 间 无 关 的 , 即 ,/_ — f RA, — h. 
式 (3 -17) 和 式 (3 - 18) 所 采用 的 特殊 解析 形式 代表 了 需要 状态 矢量 估计 
信息 的 系统 完整 建 模 。 在 贝 叶 斯 意义 上 ,对 x, 基于 完整 观测 集合 的 估计 zi,。 可 以 
通过 把 问题 转化 成 条 件 后 验 密度 p(x, | zl,,) 的 估计 问题 来 加 以 理解 ,本 书目 的 
在 于 提出 在 这 种 条 件 分 布 和 后 验 分 布下 与 x, 相关 时 刻 递 推 贝 叶 斯 估计 方法 。 
对 于 后 验 分 布 p(x, | Zan) ,将 式 (3 - 1) 的 贝 叶 斯 法 则 改写 为 
A PÈ Zin | x,)p(x,) _ 
p(x, l Zia) = P. (3 - 19) 
后 验 概 率 密度 函数 p(x, | zi,,) ,是 x, 的 概率 密度 函数 ,条 件 是 所 有 的 观测 值 , 包 
括 当前 的 观测 ,因为 集合 |z,, ) 可 以 写 为 (z, 2,41 , 式 (3 -19) 可 以 扩展 和 分 解 
为 以 下 形式 , 即 
_ p(z, zal x,)p(x,) B 
PX l ia) = CC po 
将 式 (3 - 3) 代 和 人 分 子 和 分 母 ,变换 为 
p(z, | Zin- Pia | x,)p(x,) 
P(X, | Zin) = p(G, | zi a ) PÈZ- ) Urso 
将 贝 叶 斯 法 则 (3 — 1) 应 用 到 pz... 1 x) ,化 简 结果 方程 ,得 
_ plz, | zi a X, )p(x, | zia )p (Zi n-1 )p(x,) 
pa, | ta) j p(z, | Bleni )p(Z;.,-1)p(*,) 
- p(z, | zy, Xx,)p(x,] Zi,n-1) 


plz) 2,4) (3 -22) 
i p(z,| Zina) (3 -23) 


HP p(z, | z,,4x,) > plz, | x, ) ,由 式 (3 - 18) 可 知 ,t, 时 刻 的 观测 值 不 依赖 于 
t, 时 刻 的 观测 值 。 

最 后 创建 一 个 完全 递 推 形式 的 条 件 概率 密度 函数 方程 -Chapman - 
Kolmogorov 方程 提供 了 先 验 概 率 密 度 p(x, | Zi) 与 后 验 密度 之 间 的 联系 , 即 


p(x, | Zia) - [pCx, | X ad 1-1 )p(x, , | Zin- )dx , 


= pz, | £ p(s ,12,,4)dx, , (3 - 24) 


由 式 (3 - 24) 和 式 (3 - 23) 可 知 ,建立 起 前 一 个 后 验 概率 p(x,_, 1 Zina) 和 当前 
后 验 概率 p(x, | zi,,) 之 间 的 递 推 关系 ,需要 由 p(x, | x, ) 给 定 预测 密度 和 似 
PR PRR p(z, | X,,) 。 

在 式 (3 - 24) rB ,p(x, | Xina) 替换 了 p(x, 1 x, zi) ,因为 预测 密度 是 
由 式 (3 - 17) 动态 方程 定义 的 ,不 依赖 于 zl,,:。 式 (3 - 17) 表明 ,x, 的 密度 依赖 
Fx, My, MR v,_, 均值 为 零 ,那么 x, 的 平均 值 就 是 f(x,_, ,u,) 的 平均 值 。 
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另外 ,由 式 (3 - 18) 可 知 ,z, 可 以 被 看 作 一 个 随机 变量 , 它 的 当前 值 依赖 于 
x,o 因 此 , 式 (3 - 1) 定义 了 规范 化 的 似 然 函 数 p(z, 1 x.)/p(z,) H K(3 - 1) 可 
知 ,由 于 p(x) #lp(x | z) 是 概率 密度 ,p(z, | x,)/p(z,) 一 定 也 是 概率 密度 。 因 
此 ,p(z,) Æ p(z, | x,) 的 规范 化 函数 。 

图 3 -1 给 出 了 为 得 连续 后 验 密度 而 进行 的 贝 叶 斯 递 推 过 程 的 一 次 迭代 。 递 
推 过 程 从 p(xo) 开始 ， x 相关 的 最 先 观测 的 概率 密度 函数 。 

一 步 贝 叶 斯 递 推 估计 





图 3 -1 一步 递 推 贝 叶 斯 后 验 密 度 估计 过 程 


在 理论 上 , 式 (3 717) 中 的 马尔 可 夫 过 程 可 当 作 马尔 可 夫 链 x),, = dixe 
x,| 的 一 部 分 ,预测 和 更 新 方程 ,分 别 为 式 (3 -24) 和 式 (3 - 23 ) ,写成 马尔 可 夫 
fH?) 形式 ,由 于 本 书 主要 涉及 跟踪 应 用 ,将 马尔 可 夫 链 简化 为 一 阶 马尔 可 夫 过 
fe ,推导 过 程 中 省 略 马尔 可 夫 链 符号 ,支持 一 步 递 推 公 式 。 对 于 动态 模型 高 阶 马 
尔 可 夫 过 程 情况 ,如 时 间 序 列 的 数据 分 析 ,通常 可 以 创建 扩展 状态 矢量 来 得 一 阶 
马尔 可 夫 过 程 。 


3.4 ”基于 状态 均值 和 协 方差 的 递 推 贝 叶 斯 估计 


这 里 引入 一 个 符号 x,, ,在 本 书 的 后 续 部 分 将 使 用 这 个 符号 。 令 x, 的 估计 以 
截至 当前 时 刻 c, 的 所 有 观测 为 条 件 , 则 有 


fn = Elx, | z! a [xa | z,,,)dx, (3 -25) 
iX FE x, 的 估计 使 用 包括 当前 t, 时 刻 在 内 的 所 有 观测 值 那么 
Šua = Elx, l Zian} = [xp x, | Zin yd (3 - 26) 
对 x, 的 预测 是 基于 除 当前 观测 之 外 的 所 有 观测 , 即 
Suet = Ely | | (3 -27) 


对 协 方差 矩阵 做 同样 运算 ,得 
PEL = € itz, =$) (x. 一 Xin) | Zi in| 
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š J G. -X,,,) (x, - Xin) p(x, | Zin) dx, (3 - 28) 
R^; 


协 方差 的 预测 形式 如 下 
| (3 - 29) 


= | 一 Lan) GE, -Xx,,) p(x, | Zin) dx, 


3.4.1 ”状态 矢量 预测 
将 Chapman - Kolmogorov 方程 (3 - 24) 代入 式 (3 - 27) 和 式 (3 - 29) 


得 Enna = | [xp za )p(x., l Lint)dr dr, (3 - 30) 
RR" 
和 Puasa = J | “=, -Šani ) (x, = l P )'p(x, | Ba? Ë 
RR" 
p(x, , | Zi..-1) dx, ,dx, (3 - 31) 


根据 系统 动态 方程 (3 - 17) , 式 (3 - 30) 可 改写 为 
£a = | [Uem tv p(x x...) ° 
ee, | z,,,) dx, _ dx, 
= [GG +u, alpina! Zi Ox, ° 


fo, | x, ) dx, (3 - 32) 
R". 

= ffa Le, )p(x | | Ehina )dx, , + u, + 
Rnx 


[rap | Zin dr , Gi = 335 


ERG - 32) 推导 到 式 (3 - 33) 的 过 程 中 ,密度 函数 的 积分 是 涵盖 整个 支撑 区 
域 的 密度 , 且 其 值 总 是 为 1, 因 此 积分 值 jp(x, | x...) de, = 1。 方 程 (3 - 33) 包括 


两 个 密度 加 权 积分 ,除非 在 特殊 情况 下 ,否则 一 般 无 法 用 解析 方法 求解 。 如 果品 
F v, , 均值 为 零 ,需要 一 个 无 偏 估 计 器 ,第 二 个 积分 项 简化 为 零 。 
类 似 地 , 式 (3 - 31) 化 简 为 


Pa = [ ha) +u, = Kasa fia, ) + u, -Xaalo ° 
R" 


p(x, | Z... -i) dX, = 


[UG +u, En eC, | Zini) de, — 


R^: 
(3 - 34) 
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Ika [fat (x. ) *2, = t P ]'p(x, , | Zu )dx, , $ 


R", 


fv. tap (x... | Zia )dx , 
Ru, 
定义 噪声 协 方差 为 
Q 4 foaire, | Zi en-1 )dx, , (3 = 35) 
R^. 


Ef, (x, ,u,) 与 mw- 不 相关 , 式 (3 7 34) 中 间 两 个 积分 化 简 为 零 ,密度 加 权 
预测 方差 则 变 成 


Pa | UG +8, = Saa lta) +u,- Zaal” š 


p(x, | z,,4)dx, , + Q (3 - 36) 

因此 , 式 (3 - 33) 和 式 (3 - 36) 状态 矢量 的 一 阶 和 二 阶 矩 的 预测 估计 是 基 

于 除 最 近 一 次 观测 外 的 所 有 观测 为 条 件 的 。 现 在 需要 的 是 给 定 当前 观测 值 z, 条 

件 下 的 一 种 递归 更 新 这 些 预 测 矩 的 方法 .直接 得 到 后 验 密 度 的 前 两 阶 矩 ,而 无 需 
评价 式 (3 - 26) 和 式 (3 - 28) 。 


3.4.2 ”状态 矢量 更 新 


对 于 所 有 一 般 的 密度 ,卡尔 曼 滤波 器 提出 一 种 不 采用 贝 叶 斯 法 则 ”| 的 滤波 
器 ,假定 系统 状态 x, 可 以 按 次 序 得 估计 , 仅 更 新 一 阶 和 二 阶 预 测 矩 .具体 而 言 ， 
假定 一 阶 矩 的 更 新 估计 是 线性 依赖 于 当前 观测 值 , 即 

xX, = ÁZ? +b (3 -37) 

式 中 :z 为 最 新 的 含 曲 观测 值 ;4 和 4b 为 待 确定 的 未 知 数 。 

根据 当前 观测 值 zs (zs 为 随机 变量 ) ,随机 变量 x, 最 佳 的 线性 估计 (从 最 小 
均 方 误差 意义 上 说 ) 需 满 足 如 下 条 件 : 

(1) 条 件 估计 具有 无 偏 性 , 即 每 个 元 素 的 估计 误差 式 (3 - 9) 均值 为 零 , 例 
WELE l Zini) =0 e R"; 

(2) 每 个 元 素 的 估计 误差 与 所 有 元 素 的 观测 值 不 相关 , 即 Ejez” | z... | 
s0 cR", 

注意 ,上 述 两 个 条 件 都 建立 在 估计 误差 之 上 ,而 不 是 在 状态 矢量 或 观测 矢量 
之 上 。 同 时 ,后 一 个 条 件 要 求 估 计 误 差 是 与 观测 值 正 交 。 

根据 式 (3 - 9) P e 的 定义 和 方程 (3 - 37) ,运用 条 件 (1) ,得 


Elé | fol = €l x, -= Kun | pe 
= Elx, | Zinat - (A Elza | zal +D) 
= X... — AZ... -b=0 (3 - 38) 


^ 


b =- Z... = AZ...) (3 -39) 
由 式 (3 - 37) 得 
= PR * A(z, = Zia) (3 - 40) 
条 件 (2) 是 正 交 性 条 件 ,需要 
EVEZ l z... = Ell, - X,, — AZ. -anad Jet (3-41) 
H.x... Mun EA za 为 条 件 。 因 为 ElEl =0,Elez | = €l (z - 
Zn) | ,因此 式 (3 - 41) 可 改写 为 


£|e;z, | Z = €l[x, - x4,4 = A(z; -24,4)]* 
(z;-Zz,,4)'| Zi 
= Pu 一 AP =0 (3 - 42) 
求解 最 后 一 个 方程 中 的 4 值 ,得 
A = K, £ Pini lPi] (3 - 43) 
用 式 (3 -37) 组 合式 (3 -39) 和 式 (3 - 43) ,得 线性 MMSE 估计 器 , 即 
XQ, = Xue + K (2; — Lun) (3 - 44) 
这 里 观测 值 的 预测 误差 被 称 为 新 息 (innovations) , 即 
€; BZ — Zu. (3 - 45) 


更 新 后 的 协 方 差 可 以 由 下 式 得 , 即 
Pin = Elee | £l 
= £ (x. — Za.) (z. 2L d Zit 


= €! [x, — Xn a K, Az, 7fa»4)] N 


[x, = Xuan = K, (z fas] 1 Zi-il (3 - 46) 
经 过 一 些 代数 运算 , 式 (3 - 46) 可 变 成 
= a E KS, UR (3 - 47) 


如 上 所 述 , 式 (3 — 44) 和 式 (3 - 47) 以 及 卡尔 曼 增益 定义 (3 - 43) ,都 是 使 
用 最 新 的 观测 值 来 更 新 由 式 (3 - 33) 和 式 (3 -36) 得 的 预测 估计 的 卡尔 曼 滤 波 
器 更 新 方程 .但 是 ,上 述 方程 需要 额外 的 预测 密度 加 权 积 分 z ,1 Pa. AP 
的 计算 。 通 常 ,这 些 预测 值 相 关 预 测 估计 定义 如 下 , 即 


inel À elz, | x,, zal (3 - 48) 
EL A e| (z, - aat) (2, ud TNCS iE X, Epis | t3 - 49) 
Pact & 21(%, -x,,4)(£, = Žan) | Bijil (3 - 50) 


用 这 些 参 数 来 推导 式 (3 - 33) MR - 36) , WERE 
Evi = [h(x pCa, | 21,1) dx, + 
Rn. 


f wo. 1 21) dx, (3 - 51) 
Rnx 
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Pj [ [h(x,) - Zini LA, (x) = East] s 
R”: 


p(x, | z,,,)dx, +R (3 - 52) 
Pina = [[x. - š... M(E) - š... 1 - 


R”: 


p(x, | Zini) de, (3 — 53) 


RÀ | wwo, z ) dx, (3 - 54) 
R^; 


式 (3 - 52) 中 假定 h(x,) 独立 于 w, ,因此 交叉 项 积分 为 零 ,通常 假定 w, 均值 为 
零 ,因此 式 (3 - 51) 的 第 二 个 积分 项 积分 值 为 零 。 
图 3 -2 给 出 了 任意 分 布下 递 推 点 估计 过 程 的 总 体 框 图 。 





图 3 -2 弟 推 点 估计 过 程 的 总 体 框图 


3.5 一 般 估计 方法 讨论 


本 章 给 出 了 贝 叶 斯 估计 的 两 个 主要 结果 ,在 概率 密度 的 形式 和 动态 方程 或 
观测 方程 的 线性 度 上 几乎 没有 任何 限制 ,第 一 个 结果 (利用 式 (3 - 23) 和 
式 (3 - 24) 得 ) 建立 前 一 个 的 后 验 概率 p(x, 1 z... I) 和 当前 后 验 概率 p(x, | 
Zin) 之 间 的 递 推 关系 ,需要 给 出 预测 密度 px, | xna) 和 似 然 函 数 p(z, | x.) ç 

在 许多 感 兴趣 的 问题 中 ,这 些 密度 的 递 推 蒙特 卡 洛 评价 提供 了 以 所 有 观测 
值 为 条 件 ,状态 矢量 可 能 的 位 置 和 传播 的 可 视 化 洞察 .本 书 第 HU 部 分 将 扩展 这 
些 蒙特 卡 罗 佑 计 方 法 使 其 适用 于 非 线性 系统 和 非 高 斯 密度 情况 (线性 和 高 斯 情 
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况 作 为 一 个 子 集 被 包含 在 内 ) ,此 外 ,提出 的 蒙特 卡 洛 方法 可 以 评价 密度 加 权 积 
分 ,其 中 x, 的 蒙特 卡 洛 采样 可 以 用 来 生成 密度 函数 ,简化 了 采样 点 加 权 和 的 积 
分 ,后 面 这 些 方法 被 称 为 粒子 滤波 器 。 

本 章 提出 的 第 二 个 估计 过 程 是 一 个 通用 的 方法 , 递 推 估计 以 所 有 观测 值 为 
条 件 的 前 两 阶 矩 的 状态 矢量 。 这 种 方法 使 用 密度 加 权 积 分 预测 估计 x... 
PT, a Banai Pan 8 Pia 分 别 由 式 (3 - 33)、 式 (3 - 36)、 式 (3 - 51). 
式 (3 - 52) 和 式 (3 - 53) AM. 然后 是 卡尔 曼 滤 波 更 新 方程 式 (3 - 43). 
式 (3 -44) 和 式 (3 - 47). 

目前 提出 的 方法 都 是 通用 的 ,对 动态 过 程 或 观测 过 程 的 形式 和 概率 密度 的 
形式 都 没有 限制 .本 书 第 H 部 分 致力 于 高 斯 密度 下 的 点 估计 方法 ,在 这 个 高 斯 
假设 基础 上 ,导出 本 书 中 所 有 卡尔 曼 滤波 器 的 变型 .卡尔 曼 滤波 器 的 变型 主要 分 
为 三 种 类 型 ,线性 卡尔 曼 滤 波 器 、 扩 展 卡尔 曼 滤波 器 和 Sigma 点 卡尔 曼 滤 波 器 
(数值 计算 高 斯 加 权 积 分 ) 。 
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第 4 章 
实用 案例 :初步 探讨 





本 书包 括 4 个 实用 案例 ,用 来 举例 说 明 第 H 和 第 HL 部 分 提出 的 跟踪 滤波 器 
的 应 用 和 性 能 ,本 章 给 出 第 一 个 案例 ,演示 通过 定向 频率 和 测 距 (DIFAR) 声 纳 
浮标 场 进行 船舶 跟踪 的 问题 .这 是 一 个 特别 有 趣 的 问题 , 当 输 入 信和 号 为 高 信 噪 比 
时 ,每 个 浮标 的 方位 观测 值 的 噪声 是 高 斯 分 布 的 ;而 当 输 入 信号 为 低 信 噪 比 时 ， 
噪声 是 非 高 斯 分 布 的 .这 个 案例 可 以 检验 低 信 噪 比 条 件 下 跟踪 滤波 器 的 效用 ,此 
时 的 观测 噪声 是 非 高 斯 分 布 的 。 在 第 TEORIA HL 部 分 ,利用 这 个 案例 说 明 多 种 跟 
踪 滤 波 器 都 可 以 用 在 DIFAR 问题 上 ,并 为 每 种 估算 方法 提出 实现 细节 , 标 绘 出 
跟踪 输出 结果 和 真实 航 迹 , 最 后 评价 所 有 跟踪 算法 的 相关 跟踪 性 能 度量 值 。 

本 书 第 IV 部 分 将 讨论 其 它 的 案例 分 析 ,包括 利用 雷达 观测 数据 跟踪 三 维 空 
间 中 运动 的 物体 ;根据 视频 观测 跟踪 下 降 炸 弹 ( 摄影 测量 法 ) ;以 及 在 摄影 测量 
法 跟踪 系统 中 使 用 多 种 跟踪 滤波 器 数据 融合 ,这 些 案例 分 析 的 范围 从 相对 简单 
的 DIFAR 纯 方 位 跟踪 问题 ,到 非常 复杂 的 需要 高 维 状态 矢量 、 观 测 矢 量 和 高 数 
据 速率 的 问题 .对 于 某 些 问题 ,所 有 的 跟踪 滤波 器 都 能 得 基本 相同 的 结果 ;而 对 
其 它 一 些 问题 ,有 些 跟踪 滤波 器 则 无 法 使 用 ,因为 这 些 跟踪 滤波 器 无 法 满足 问题 
的 内 在 要 求 。 


41 仿真 .估计 与 评价 过 程 


为 特定 的 跟踪 问题 场景 编写 软件 代码 ,是 一 项 很 艰巨 的 任务 。 在 图 4 - 1 中 ， 
给 出 了 必须 解决 的 问题 过 程 框图 ,首先 需要 建立 一 个 场景 模拟 器 ,生成 待 估计 状 
态 的 真实 数据 ;然后 将 这 个 真实 数据 集 转 换 为 一 组 真实 观测 值 ;最 后 添加 噪声 。 

例如 ,在 三 维 稍 卡 儿 坐 标 中 跟踪 一 辆 车 -首先 要 生成 车 辆 在 笛 卡 儿 坐 标 中 的 
真实 位 置 和 速度 分 量 (也 称 作 真实 轨迹 ) ,如 果 观 测 值 来 自 于 雷达 , 笛 卡 儿 坐 标 
真实 轨迹 必须 从 笛 卡 儿 坐 标 变换 到 球 坐 标 ,生成 不 含 噪 声 的 球 坐 标 真 实 轨 迹 , 包 
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FEW BE .方位 ,高度 和 分 别 的 速度 。 然 后 ,把 具体 的 雷达 观测 值 ( 如 测 距 ,方位 \ 高 
度 、 和 /或 多 普 勒 距离 变化 率 ) 从 球 坐 标 真实 值 中 分 离 出 来 ,在 使 用 跟踪 滤波 器 
处 理 真实 雷达 获得 的 数据 时 ,最 后 的 仿真 步骤 可 以 省 略 。 仿 真 步 骤 主 要 是 用 来 测 
试 和 调试 估计 算法 的 执行 情况 ,比较 评估 跟踪 算法 的 性 能 。 

在 第 二 个 方 框 中 ,通常 是 测试 蒙特 卡 洛 运动 跟踪 算法 ,并 在 观测 值 中 加 入 品 


E ,将 含 品 观 测 值 作为 跟踪 滤波 器 的 输入 。 所 研究 场景 仿真 器 
的 跟踪 滤波 器 生成 以 时 间 为 变量 的 跟踪 状态 矢量 ， —— s ER 
以 及 跟踪 状态 误差 协 方差 估计 函数 。 设置 动力 学 噪声 参数 


最 后 一 个 方 框 中 对 跟踪 算法 的 性 能 进行 了 评 RAR MME 
估 。 对 于 仿真 观测 值 ,可 以 将 真实 跟踪 数据 和 跟踪 
估计 用 在 一 系列 蒙特 卡 洛 运行 中 , 生成 均 方 根 
(RMS) 误差 并 将 均 方 根 误差 与 Cramer - Rao FA 蒙特 卡 洛 估计 /跟踪 
(CRLB) 进行 比较 。 当 使 用 实际 系统 数据 时 ,通常 无 | | ee 
法 得 到 真实 航 迹 ,可 以 使 用 估计 的 跟踪 误差 协 方差 


来 生成 跟踪 估计 的 误差 椭圆 。 

整个 仿真 中 的 每 个 流程 框 可 以 独立 开发 ,因为 性 能 评价 
在 多 数 情况 下 ,这 些 流程 相对 具体 跟踪 算法 具有 不 mene 
变性 。 计算 Cramer-Rao 下 界 





下 节 将 讨论 DIFAR 跟踪 场景 仿真 器 的 算法 基 
础 ,并 根据 具体 问题 分 析 算法 的 细节 。 对 于 第 H 和 ”图 4-! 跟踪 算法 的 
第 H 部 分 提出 的 每 种 航 迹 估计 方法 ,作为 每 种 算法 UPC SP S 
的 具体 应 用 实例 ,将 给 出 DIFAR 问题 的 航 迹 估计 
结果 ,在 14. 5 节 对 各 种 高 斯 估计 方法 处 理 DIFAR 问题 的 性 能 进行 比较 , 非 高 
斯 噪声 情况 下 粒子 滤波 器 估计 方法 的 性 能 比较 将 在 本 书 第 H 部 分 末尾 加 以 
阐述 。 


42 利用 DIFAR 浮标 场 进行 匀速 运动 目标 跟踪 的 场景 仿真 


这 里 的 DIFAR 场景 包括 匀速 通过 DIFAR 浮标 场 的 船舶 ,使 用 的 坐标 系 是 二 
维 笛 卡 儿 坐 标 系 , 以 浮标 场 的 中 心 为 坐标 原点 ,x 轴 指 向 东 ,y 轴 指 向 北 。 


4.2.1 船舶 动态 模型 


用 状态 矢量 x(1) 来 描述 上 时刻 船 舶 的 运动 ,矢量 由 二 维 坐标 下 的 位 置 和 速 
度 分 量 构成 

x(t) =[r(t) v P(t) v] (4 - 1) 

AEH ir (2) P(t) | vv | 分别 为 笛 卡 儿 坐 标 下 的 位 置 和 速度 分 量 。 这 里 采 
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用 了 匀速 运动 模型 , 速度 分 量 独 立 于 时 间 。 假定 场景 开始 时 船舶 的 位 置 在 
[r Cto) mn (to) | ,那么 在 时 间 上 时 ,场景 中 船舶 的 位 置 分 量 可 写成 


r'(t) 2 (to) +0°t (4 - 2) 
r (t) 2r(t,) *vt (4 -3) 
用 矢量 矩阵 表示 ,可 改写 为 

x(t) = F,x(to) (4 - 4) 

P 0, 
Ẹ, = š 4-5 
[s MI (4 - 5) 

L š 
F,, = 4-6 
"Loi EM (4 - 6) 


4.2.2 多 浮标 观测 模型 


假设 有 以 个 浮标 随机 分 布 在 以 原点 为 圆心 半径 为 R,, 的 区 域内 ,第 m 个 浮 
标 位 于 固定 位 置 | x, y, | 。 当 然 ,在 实际 浮标 场 跟 踪 运 动 中 , 由 于 水 流 漂移 和 波 
浪 作 用 ,这 种 在 跟踪 估计 时 间 帧 中 浮标 位 置 固定 的 假设 条 件 不 可 能 实现 ,为 便于 
算法 证 明 , 这 里 假定 浮标 位 置 固定 。 

将 方位 定义 为 相对 正 北 (y 轴 方向 ) 的 一 个 顺 时 针 角 度 , 可 以 将 :时 刻 第 mm 个 
浮标 与 目标 之 间 的 方位 角 写 作 

aft 4) — x, 
0, (t) = tan k'as A (4 -7) 

Xp:-mTEO,.() mm. 


4.2.3 场景 属性 


场景 生成 的 开始 两 步 为 : 

(1) 利用 式 (4 -2) 和 式 (4 -3) 生成 均匀 间隔 时 间 集 合生 nm = 0,10, IN] 
上 的 真实 航 迹 ; 

(2) 进行 笛 卡 儿 坐 标 至 极 坐 标的 变换 ,用 式 (4 -7) 对 场景 中 的 时 间 帧 生成 
没有 噪声 的 观测 值 。 

图 4 -2 给 出 在 以 (0,0) 为 中 心 的 浮标 场 中 ,目标 船舶 跟踪 的 几何 原理 。 

图 4 -3 给 出 船舶 的 真实 航 迹 、 真 实 方位 与 两 个 DIFAR 传感器 之 间 的 关系 。 
图 4 -3(a) 显示 了 一 小 部 分 船舶 从 左下 角 向 右上 角 运 动 的 真实 航 迹 ,还 给 出 了 
原点 周围 随机 分 布 的 15 个 浮标 的 浮标 场 的 示例 .图 4 -3(b) 给 出 了 船舶 与 左 图 
中 分 别 标记 为 * 和 x 的 两 个 浮标 之 间 的 方位 角 。 图 中 浮标 相对 于 坐标 原点 的 距 
离 和 方位 角 的 分 布 服从 下 式 

Pm = RQ,*u,,m = 1 M 
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7( 北 ) 


X Ok) 





[rxtt), ry] 
目标 船舶 


图 4 -2 DIFAR 浮标 几何 原理 
9, = U( - 7,7) |, m = LM 


y 轴 位 置 /n mile 
° 
9 e 
coo 
时 间 /s 
Š 


0 
一 ] 0 l 2 -200 -100 0 100 200 
x 轴 位 置 /n mile Ji fi /C) 
(a) (b) 


4-3 利用 DIFAR 浮标 场 和 两 个 浮标 的 联合 方位 角 进 行船 舶 跟踪 


JOP R ma WH 152. 4m(500ft) [u,, ~ UCa,b) RAN un EMAKE (a,b) 中 均匀 分 布 
得 ] ,将 极 坐 标 表示 的 浮标 位 置 集合 |p, Onim = 1,… MI 用 下 式 变换 为 笛 卡 儿 
MERGE Ix, y, sm = 1,…,M| , 即 
x, =p, sin 9, (4 -8) 
y, = Pm cos Òn (4 -9) 
4 -3(b) 显示 了 方位 角 相 关 的 两 个 标记 浮标 ,一 个 浮标 在 船舶 航 迹 的 一 
侧 。 由 图 所 示 ,浮标 方位 为 东 ( 右 侧 ) 时 ,船舶 航 迹 为 0°; 当 浮标 方位 为 西 时 , 船 般 
航 迹 为 - 180°, 然 后 跳 转 到 180°, 
方位 为 西 的 浮标 经 过 了 所 谓 的 相位 卷 裙 ,相位 角 跳 转 360°, 第 6 章 将 给 出 在 
所 有 跟踪 算法 中 相位 卷 宰 所 带 来 的 问题 ,为 使 跟踪 器 能 正常 工作 ,必须 对 其 加 以 
修正 -类 似 的 相位 卷 袜 问题 也 发 生 在 许多 不 同 领域 中 的 估计 方法 的 应 用 上 ,为 得 
有 效 的 结果 ,必须 加 以 解决 .列表 4 -1 中 给 出 了 一 个 MATLAB 子 程序 ,可 以 用 来 
生成 船舶 在 笛 卡 儿 坐 标 和 球 坐 标 下 的 真实 航 迹 , 得 一 组 随机 的 浮标 位 置 。 
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列表 4 - 1 生成 真实 航 迹 及 浮标 位 置 
function [x_true,r_true,f,s,SNR,BT,SensorLocations, 
R,theta_sigma,t] = DifarScenarioGenerator(Stream,... 


N,M,Rmax,del_t,speed,heading,y_init) 


global deg2rad hours2seconds 
measurement, sigma = 3 * deg2rad; 


theta sigma - 1; 


t = del_t*(1;N); 

speed = speed/hours2seconds; 
heading = heading * deg2rad; 

vx init = speed*sin(heading); 


vy_init = speed *cos(heading); 


x init = y init*(vx init/vy init); 


x true init = [x init;vx init;y init;vy init]; 


r m = Rmax x rand( Stream,M,1); 

theta m = 2*pi*(rand(Stream,M,1) - 0.5); 
xx -rm.*sin(theta m) + Sensor Offset(1); 
yy -r m.*cos(theta m) + Sensor Offset(2); 


Sensor Locations = [xx yy]; 
R = diag(measurement sigma - 2 * ones(M,1),0); 


[x true,f,s,SNR,BT] - ... 
Constant, Velocity Kinematics Difar(Stream,... 


N,t,x true init); 


[r true] = Cart2Polar(x true,Sensor Locations,N,M); 


本 书 第 H 和 第 HI 部 分 将 提出 一 些 基于 一 组 含 噪 观 测 值 进 行船 舶 航 迹 顺 序 
(时 间 上 ) 估计 的 算法 .对 于 每 种 估计 方法 ,利用 DIFAR 观测 值 作为 滤波 跟踪 估 
计 的 输出 结果 。 


4.3 DIFAR 浮标 信号 处 理 


DIFAR 传感器 (一 种 矢量 传感器 ) 是 一 个 被 动 接收 器 ,如 图 4 -4 所 示 ,对 远 
处 船舶 辐射 的 宽带 信号 进行 运算 ,将 其 变换 为 含 噪 船舶 方位 信号 。DIFAR 浮标 
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由 3 个 独立 的 传感器 组 成 :全 向 水 听 器 、 东 西 偶 极 子 水 听 器 和 南北 偶 极 子 水 听 
器 .两 个 偶 极 子 水 听 器 可 能 不 按照 规定 的 方向 排列 ,但 将 其 输出 进行 电子 混合 ， 
可 以 得 两 个 偶 极 子 在 正确 方向 的 输出 ,后 面 不 会 进一步 讨论 这 个 重 定位 过 程 , 因 
为 它 与 跟踪 问题 无 关 , 通 常 假定 每 个 通道 的 输出 都 是 沿 笛 卡 儿 坐 标 轴 方 向 的 。 

在 每 个 模拟 DIFAR 传感器 输出 中 , 需要 生成 含 噪 的 方位 观测 值 以 实现 仿 
真 , 如 图 4 - 4 所 示 , 对 于 第 m 个 传感器 , 每 个 通道 输出 的 真实 信号 如 下 式 所 
示 , 即 


zns(t) = s(t)cos@ + ny (t) (4 - 10) 
zw (t) = s(t)sin0 + ng (t) (4-11) 
zo ($) = s(t) #nolt) (4 - 12) 


式 中 ;s(1) 为 DIFAR 浮标 的 输入 信号 。 





图 4 -4 DIFAR 传感器 信号 处 理 
互相 关系 数 C,(t) 和 C, (z) 定义 为 
C,(t) =a: (t) z(t) 
= s2(t)cos0 + s(t)no(t)cosð + 
S(t) nyo (t) + ny(t)no(t) (4 - 13) 
C,(t) = zew(t) X zo (t) 


= s (t)sin + s(t)no(t)sing + 


s(t)new(t) + ngy Ct) no(1t) (4 - 14) 

假定 9 随时 间 变 化 缓慢 ,C(t) 和 C CO). 的 期 望 值 可 写 为 
E{C,(t)} = Els? (t)! coso (4 - 15) 
E{C,(t)| = Els?’ (1)] sing (4 - 16) 


假定 信号 和 噪声 分 量 是 零 均 值 高 斯 过 程 , 与 传感器 和 时 间 无 关 。 这 样 , 由 

式 (4 - 17) 得 方位 角 为 
-1 E\C,(t)} 
E\C,(t) | 
为 得 EC (t)! MIE} C,(t) } 的 估计 值 , 对 zvs(:) Zew(t) All zo (t) 进行 采样 ， 
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0(t) = tan (4 - 17) 


把 时 间 划 分 成 长 度 为 了 的 段 , 令 
T, = ty +(i-1)8, - 26,8 = Lon, Nin = 1,2, m iq = 1,2, 


8 = T/N 
(4 - 18) 
例如 ,可 写成 
se. te ee 
I l 2 t 
Ta = t, + ó, - 38 
5 Sue 1) - 38, " «(3 -1)& 
ra = b - 28, (4 - 19) 
fE t, FA, ei C (t) HL 81 C, (5) | 可 由 式 (4 -20) 和 式 (4 - 21) 估计 得 。 
tat, 
ElCi(t,)} = f a(t)z0(t) di (4 - 20) 
NA 
elC (t,)} = | zew (£) zo (1) dt (4 - 21) 
H Weiner - Khinchine I! , 式 (4 - 20) Ast (4 - 21) 可 写 为 
e1€,(t,)) = [Ya G...) Yo Gu f df (4 - 22) 
eC Q,)] = [Ya Ct, f Yo (te hid of (4 - 23) 
这 里 * XondtguusTt, AA 
Yas (tifi) = S(t,,f,) cos0 + Nyc (t, A.) (4 - 24) 
Ye (t, A) = S(t, f,) sind + Mew (t, A.) (4 - 25) 
Yo(t, f.) = S(t, f) + Nolt, A) (4 - 26) 
对 于 采样 数据 ,时 域 方程 (4 - 20) THA = 21) 变 成 
€{C,(t,) | - EY s B) lað.) (4 - 27) 
&1C,(1,)] = LY s (08,206 08,) (4 - 28) 
其 中 
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N +2 
: 2 
在 频 域 上 ,对 集合 1zvs(i) ,zew (0) ,zo(t)| 中 的 入 个 采样 点 进行 快速 傅 里 叶 
变换 (FFT) ,得 频 域 采样 集合 | Ya Ct, Se) ,Yew Ct, fi) s Yo Cn fi) 1 ,进而 导出 





(4 - 29) 


a 
l> 
~ 

| 


N/2 

GD) = e Y us, i Yd Cs f) (4 - 30) 
8 Ne 

#|C,Gt,)1 = Wa Ye tu Aa) Yo (tu f) (4 - 31) 


3&4 -1 给 出 了 根据 船舶 真实 航 迹 生 成 每 个 DIFAR 传感器 含 噪 方位 观测 值 
的 流程 ,列表 4 -2 中 给 出 生成 含 噪 宽带 信号 的 MATLAB 程序 片段 ,列表 4 -3 为 
实现 表 4 - 1 所 列 流程 的 MATLAB 子 程序 。 
X4-1 ARS DIFAR 观测 矢量 的 流程 


8,0) = an (568 和) 


Yys,m (t, Si) = SC, fp) cos On 
+ Nys (ty Se) 
Yew.m(ta dk) = S(t, fi sin Om 
+ New (t, ft) 
Yo(4, Si) = SC) + No (ta Ae) 


Cy im (ty) = Ys (ta Sr) Yo (t, J.) 
C, im (ta) = Yew (tn Se Yo (t, fu) 
ô N/2 


&1C, ,m(t,)| ` N/A pag Cin CR) 


6 N/2 
€1Cy n (ty) | = =n ees C, nt.) 


C 
— ELM Eres 


列表 4 -2 fr K ^ie Ez rB ^E XN 4 RS Pr 3 FEBJ MATLAB 代码 片段 
f = 300:15;400; 
f_target = [10 20 50 100 200 400 1000 4000]'; 





SPL_target = [20;20;20;20;20;20;20;20]; 


SPL = interpla(log10(f target),SPL target,log10(f")); 
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spl = repmat(10.^(SPL/10),1,N); 
SNR = spl(1); 
K = length(f); 
BT = (f(K) - £(1)) *(t(2) - t(1)); 


SPL = interplqd(log10(f target),SPL target,log10(f')); 
spl - repmat(10.^(SPL/10),1,N); 

SNR - spl(1); 

K = length( f); 


BT = (£(K) - £(1)) *(t(2) - t(1)); 


s = sqrt(spl/2). *(randn(Stream,K,N) +... 
li*randn(Stream,K,N)); 


K = length(f); 
BT = (£(K) - £(1)) *(t(2) - t(1)); 
列表 4 - 3 按照 图 4 — 1 中 步骤 生成 含 噪 方位 测量 值 的 MATLAB 子 程序 
function [bearing_measurement] = ... 
First Sensor Measurement, Generator Difar(f,s,... 


theta sigma,theta true,N,M,Stream) 


K = length( f); 
del f - f(2) - £(1); 


NS, ms = sqrt(theta sigma) *«(randn(Stream,K,N,M) +... 
li *randn(Stream,K,N,M)); 

EW ms = sqrt(theta sigma) *(randn(Stream,K,N,M) +... 
li *randn(Stream,K,N,M)); 

O ms = sqrt(theta sigma) * (randn(Stream,K,N,M) +... 
li *randn(Stream,K,N,M)); 


Y NS = repmat(s,[1,1,M]). *... 
repmat(cos(theta true),[K,1,1]) + NS. ms; 
Y EW = repmat(s,[1,1,M]). *... 
repmat(sin(theta true),[K,1,1]) + EW. ms; 
Y O = repmat(s,[1,1,M]) «O ms; 


Cl = real(Y NS. * conj( Y. O)); 
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C2 = real(Y_EW. *conj(Y_O)); 


E_Cl = (2 *del_f /N) *permute(sum(C,),[2 31]); 
E_C2 = (2 *del_f /N) * permute(sum(C2),[23 1]); 


bearing measurement = permute(atan2(E C2,E C1),[21]); 

必须 注意 , 表 4 - 1 中 第 7 步 包 含 的 反正 切 运算 里 的 比值 是 随机 数 的 比值 . 众 
所 周知 ,两 个 独立 零 均值 变量 比值 的 分 布 为 柯 西 分 布 。 然 而 ,如 果 两 个 变量 均值 
不 为 零 ,其 比值 分 布 就 要 复杂 得 多 ,这 在 下 节 中 将 会 看 到 。 此 外 ,对 于 DIFAR 方 
位 ,反正 切 运算 的 分 布 更 为 复杂 ,所 以 不 能 期 望 DIFAR 方位 观测 值 符 合 高 斯 分 
布 。 但 是 ,可 以 期 望 分 布 是 DIFAR 浮标 输入 信号 信 噪 比 的 函数 。 为 说 明 这 一 点 ， 
4 -5 给 出 了 4 个 不 同 的 信号 信 噪 比 (DIFAR 传感器 的 输入 ) 条 件 下 ,浮标 在 
100 次 蒙特 卡 洛 运行 中 特定 时 刻 的 方位 观测 值 - 很 明显 , 随 着 信 噪 比 的 增加 ,分 
布 曲 线 变 得 更 加 突 匹 ,近似 于 高 斯 分 布 ;而 随 着 信 噪 比 的 降低 ,方位 分 布 近似 于 
[ - 180° ,180° ] 区 间 的 均匀 分 布 。 
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0 10 20 3 40 50 6 70 8 9% 100 
蒙特 卡 洛 采样 数目 


4-5 4 种 信 品 比 下 100 次 蒙特 卡 洛 运行 得 的 DIFAR 浮标 的 方位 观测 值 的 分 布 


4.4 DIFAR 似 然 函数 
如 果 假 设 DIFAR 传感器 似 然 函 数 独立 于 传感器 ,以 目标 船舶 的 位 置 为 条 件 
的 观测 矢量 似 然 函数 可 由 联合 密度 给 出 , 即 
p(z,| x,) = p(0,! x,) = [[Ip(@,..! x.) (4 - 32) 
其 中 8, = [0... "0... ]" B 





(4 - 33) 
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虽然 它 本 身 是 一 个 有 趣 的 问题 ,但 DIFAR 浮标 似 然 函数 的 完整 推导 并 不 是 
本 书 的 重点 , 感 兴趣 的 读者 可 以 在 附录 的 在 线 参考 文献 [2] 中 找到 推导 过 程 。 因 
为 在 本 书 第 亚 部 分 需要 用 到 ,这 里 给 出 似 然 密度 结果 及 其 性 质 。 

为 描述 清晰 ,省略 时 间 索 引 , 令 z, A tanb, ,根据 参考 文献 [2] 的 附录 A, 可 
以 有 


p(0,| x,) = [1 + (z,)* ]p(z, | Onm) (4 - 34) 
fra — 3v 1/2 
p(z, | @,,) -a{¥G—r),- .98 
* Ver 
= g^ ag" i 
I —- aT (4 - 35) 
| (Sl Fall | 
A 一 一 一 一 :一 一 (4 -36) 


20 -€- ug C 2 
T3 — 270,032, + O12 
w— n E c t. — Ga 
TiN -OTN N + G) T 


2010;(1- r°) 


aå (4 - 37) 


g a EOM — 1810272) + (019; - 10102002. (4 _38) 
oola - 270,02, + O02) 


(m; TE Niža)” 


ya (4 - 39) 


~ »f z2 < = ~2 2 
2( a3 -2rom,0,, + Oin) 


ó à sign[ (03m, - 10,0272) + (ai 7: - 10,02 zn] (4 - 40) 


n, à /2BTSNR cos 9i? (4-41) 
m: à /2BTSNR sin 6i? (4 - 42) 
og; A (2SNR +1)SNR cos! 6 + p(SNR +1) (4 - 43) 
ao; 4 (2SNR +1)SNR sin? 0? + p(SNR +1) (4 = 44) 
va (2SNR + = 6? sin 8? (4 - 45) 

G 05 
(x) à ——f edu (4 - 46) 

V2 = 


信 噪 比 是 单 极 通道 下 信号 与 噪声 的 比值 ,BT 为 车 辆 频率 特征 的 时 宽带 宽 
积 ,p 为 适用 于 偶 极 通道 的 噪声 增益 ( 减损 ) 因子 B(x) 与 误差 函数 相关 。 对 于 二 
维 各 项 同性 和 三 维 各 项 同性 噪声 ,p 的 取 值 分 别 为 1/2 BR 1/377 。 

注意 , 式 (4 -34) 给 出 的 似 然 函 数 实际 是 条 件 似 然 概 率 p(91 05) ,其 中 6o 为 
由 式 (4 - 7) 得 的 相对 目标 船舶 的 真实 方位 ,9 代表 计算 似 然 概率 用 到 的 方位 值 
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合 -图 4 - 6 给 出 带宽 时 宽 积 (BT) 为 10 且 信 噪 比 取 值 范围 为 ( - 10 < 
SNR < 10) 时 标 绘 出 的 (p(91 % ) 值 。 很 容易 看 出 ,对 于 低 信 品 比 情况 , 似 然 密 
度 远离 高 斯 分 布 ,这 是 由 于 它 过 高 的 拖 尾 ,还 有 它 仅 在 - mn < 9 < "区 间 范 围 有 
定义 。 





4-6 DIFAR 似 然 密 度 (BT 取 值 为 10 , 信 噪 比 从 - 10 到 + 10dB) 
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第 工 部 分 
高 斯 假设 :卡尔 曼 滤 波 估 计 器 


高 斯 噪声 :高 斯 加 权 分 布 的 多 维 积分 

线性 卡尔 曼 滤 波 器 

线性 化 卡尔 曼 滤 波 器 :扩展 卡尔 曼 滤 波 器 
Sigma 点 类 别 : 有 限 差 分 卡尔 曼 滤 波 器 
Sigma 点 类 别 :无 迹 卡尔 曼 滤波 器 

Sigma 点 类 别 : 超 球面 单 形 卡 尔 曼 滤波 器 
Sigma 点 类 别 :高 斯 — 厄 米 特 卡尔 曼 滤波 器 
蒙特 卡 洛 卡尔 曼 滤 波 器 

高 斯 卡尔 曼 滤 波 器 总 结 

卡尔 曼 滤波 器 族 的 性 能 度量 





第 5 = 
斯 噪声 :高 斯 加 权 分 布 的 多 维 积分 


a 





第 3 章 结尾 部 分 采用 一 般 概率 密度 函数 导出 了 贝 叶 斯 点 估计 方程 ,本 章 假 
定 高 斯 过 程 由 动态 方程 和 观测 方程 构成 ,在 这 种 假设 条 件 下 ,点 估计 方程 中 包含 
的 所 有 的 分 布 函数 都 是 高 斯 分 布 ,众所周知 ,高 斯 密度 用 其 前 两 阶 矩 就 可 以 完整 
表示 。 因 此 ,对 于 高 斯 过 程 来 说 ,前 两 阶 矩 的 递归 传播 估计 是 最 优 的 估计 方法 。 这 
一 部 分 的 主题 包括 多 种 算法 的 推导 ,用 来 求解 第 3 章 提 出 的 在 高 斯 密度 情况 下 
的 加 权 密 度 预测 点 估计 。 

在 第 3.4 节 ,提出 了 一 组 卡尔 曼 滤 波 更 新 方程 ,对 过 程 是 否 为 线性 以 及 过 程 
的 密度 没有 特殊 要 求 , 假 设 x,( 其 后 验 点 估计 写成 x,,) 一 定 为 最 新 观测 z 的 线 
性 函数 .点 估计 器 的 条 件 是 :估计 误差 均值 为 零 且 与 观测 矢量 互 不 相关 ( 译 者 
注 :此 处 原文 有 误 ) ,在 后 面 章节 中 将 介绍 如 何 由 高 斯 分 布 得 卡尔 曼 滤 波 更 新 方 
程 ,高 斯 分 布 能 满足 所 有 基本 假设 和 要 求 。 

以 第 3 章 提出 的 一 般 形 式 的 贝 叶 斯 点 估计 方程 为 基础 ,第 5. 3 节 主要 说 明 将 
预测 估计 方程 简化 为 高 斯 加 权 积 分 ,在 后 面 章 节 , 针 对 各 种 假设 以 及 不 同 的 动态 
函数 f(x,) 和 观测 函数 h(x, ) 的 通 近 ,用 数值 方法 求解 卡尔 曼 滤 波 器 预测 高 斯 
加 权 积 分 。 根 据 求解 方法 的 不 同 , 将 卡尔 曼 滤 波 器 分 为 以 下 4 类 : 

(1) 线性 卡尔 曼 滤波 器 。 当 动态 方程 和 观测 方程 均 为 线性 , 且 所 有 密度 均 是 
高 斯 分 布 , 积分 可 以 直接 求解 , 这 就 是 线性 卡尔 曼 滤波 器 (the Linear Kalman 
Filter, LKF) 。 

(2) 解析 线性 化 卡尔 曼 滤波 器 。 将 非 线 性 函数 展开 为 泰勒 多 项 式 , 仅 保留 线 
性 项 ,这 样 积分 又 可 以 直接 求解 ,得 一 种 与 LKF 几乎 相同 的 卡尔 曼 滤波 器 ,但 还 
要 有 一 个 额外 的 步骤 , 即 需 要 计算 每 个 非 线 性 函数 的 雅 可 比 矩 阵 。 这 种 滤波 器 包 
FED EK AR Ee BE (the Extended Kalman Filter, EKF) 及 其 各 种 变形 。 

(3) Sigma 点 卡尔 曼 滤波 器 。 将 非 线性 函数 展开 为 更 一 般 的 多 项 式 , 将 积分 
简化 为 确定 的 矢量 点 集 ( 称 为 Sigma 点 ) 的 加 权 和 。 这 类 卡尔 曼 滤 波 器 主要 包括 
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有 限 差 分 卡尔 曼 滤 波 器 (Finite Difference Kalman Filter, FDKF) 无 迹 卡 尔 曼 滤 
波 器 (Unscented Kalman Filter, UKF), 、 球 面 单 形 卡尔 曼 滤 波 器 (Spherical 
Simplex Kalman Filter,SSKF) 和 高 斯 厄 密 特 卡尔 曼 滤 波 器 (Gauss - Hermite 
Kalman Filter,CHKF ) 。 

(4) 蒙特 卡 洛 卡尔 曼 滤 波 器 。 如 果 从 高 斯 密度 得 一 组 蒙特 卡 洛 采 样 点 ,生成 
离散 密度 ,那么 积分 运算 将 简化 为 离散 随机 采样 点 求 和 运算 。 这 种 算法 得 蒙特 卡 
洛 卡 尔 曼 滤波 器 (Monte Carlo Kalman Filter, MCKF) 。 

本 书 这 一 部 分 的 很 多 材料 是 基于 Wu 等 的 论文 [1] 中 的 思想 ,Wu 等 的 论文 
简要 介绍 并 比较 了 迄今 为 止 提出 的 大 多 数 的 高 斯 加 权 积 分 解法 ,用 一 根 主线 把 
所 有 的 卡尔 曼 滤 波 算法 连接 成 一 个 统一 的 框架 。 从 这 篇 论文 当中 ,本 书 作 者 找到 
了 很 多 数值 积分 方法 相关 文献 ,加 深 了 对 求解 高 斯 加 权 积 分 的 递归 贝 叶 斯 点 估 
计 方 法 的 理解 .本 书 这 一 部 分 的 其 他 相关 参考 资料 包括 :MeNamee 和 Stenger” 
的 论文 以 及 Lemer™ 的 博士 论文 , 这 二 者 的 思想 均 源 于 Wu 的 文献 ;Davis 和 
Rabinowitz'*) „Evans 和 Swartz ^ 所 著 的 数值 积分 书籍 对 于 本 书 的 完成 起 到 重要 
EFA ; Ito Al Xiong ^! 的 论文 使 作者 对 高 斯 滤波 器 基本 概念 有 了 初步 了 解 , 对 完成 
本 书 具 有 重大 意义 。 


5.1 第 3 章 中 重要 结论 总 结 


第 3 章 的 重要 结论 总 结 如 下 ,将 动态 过 程 表 示 为 
x, = f. (x, ) + u, + v, (5-1) 
由 观测 过 程 生成 的 观测 值 为 
z, = h,(x,) + w, (5 - 2) 
基于 包括 1,_) 时 刻 在 内 的 所 有 观测 值 ,t, 时 刻 状态 矢量 点 预测 估计 由 密度 加 权 积 
分 给 出 , 即 


= JU. (Qna) +u, t vuulp(xulzuau)dx,, (5-3) 
同样 ,基于 包括 4 时 刻 在 内 的 所 有 观测 值 i, 时 刻 的 状态 协 方差 的 点 预测 估计 
由 密度 加 权 积 分 给 出 , 即 

Pia = [Usa +a, xuuallfaQn +u, = z... 1 
px tus APY. (5 -4) 


此 外 ,观测 矢量 z, 的 点 预测 估计 、z, 的 协 方差 及 其 与 x, 的 互 协 方差 由 下 式 给 出 ， 
即 


Zuni = [[h.(x,) +w,] p(x, Zin ) dx, (5 - 5) 
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Psi = [| A, Gc.) - Z... ][h,(x,) = fasal 
R^. 
p(x, | Z1.n-1) dx, +R (5 - 6) 
Pa = [tx -Žun l [h (X) A 1 


R'x 


p(x, | z,,,4)dx, kS = 73 
Q à Jvaviap Ga! Zt) d, (5 -8) 
Rn> 
R à [w,wip(x, | z,,4)dx, (5 -9) 
R"x 


5.2 ”回顾 卡尔 曼 滤 波 器 校正 (更 新 ) 方程 的 推导 


基于 所 有 条 件 密度 均 为 高 斯 分 布 的 假设 , 本 节 给 出 男 一 种 卡尔 曼 滤波 校正 
方程 (3 - 43) 式 (3 -44) 和 式 (3 - 47) 的 推导 。 贝 叶 斯 法 则 提供 的 后 验 密度 和 
x, 5j z, 之 间 联 合 密度 的 关系 为 


p(x, I^ | 2Z1:n-1 ) 


pisl sig) = PD S (5 - 10) 
定义 联合 矢量 
q à [| (5 = 11) 
Ë 
则 式 (5 - 10) 可 写 为 
AAPS = Pfs Sent? (5 - 12) 
多 元 高 斯 分 布 的 一 般 形式 可 写 为 
人 (6 s, E) à zapal- z -5)73 (t - s) } (5 - 13) 
令 式 (5 - 12) 中 的 所 有 密度 均 为 高 斯 分 布 ,得 
px, | Bos) zA(x,s £4,, PS.) (5 - 14) 
PC ea! fua) = AGIS $;, PTLu) (5 - 15) 
p(z) = A(G,:Z,4 a Puoi) (5 - 16) 


忽略 (- 1/2) 因子 ,将 式 (5 — 14) 中 的 后 验 高 斯 密度 的 指数 解析 式 中 的 指数 
显 式 地 写 为 
d à (x, —X,.)'[P2,] (x, ~ Sa.) 
= x,[ Pa.) z, - xp Pa] 34, - 
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za [P2] x, Rial Pia] Fae 
同样 , 式 (5 - 15) 等 号 右边 的 高 斯 密度 比率 解析 式 中 的 指数 为 
g = (q, = nin ES i [Po] ~ (4, = da.) = 
(z, Roa [PI] (z, - £,,4) 
由 式 (5 - 11) ,将 式 (5 - 18) 更 显 式 地 写 为 


n 7 F n-i nln-1 Prin -l By — X44 
in 7 Zain K| p Fae J E -Zu T 
(z. = Zana) p (z, ~ Bini) 
定义 式 (5 - 19) 中 分 块 矩阵 项 的 逆 为 
C, Co], [Pina Patri] 
l. ol [pe m.) 
Cy, Cy PL a Ei 
利用 第 2. 1.4 3517 ns er 
C, = [Pa nln-l ^ FLIEGT Pul 
C; =— CuPs (Ps. a)” 
C;, == CoPr (Pina) 


C, = Pa ,i "1 aUa Pa 
将 式 (5 - 20) ~ X(5 - 24) 代入 式 (5 - 19) ,展开 并 整理 ,得 


f= Ce. ^ x > Cus + C, (Z, - NM )] +e 


将 式 (5 - 17) 的 第 1 项 与 式 (5 - 25) 第 1 项 进行 比较 ,得 
PP 
将 式 (5 - 17) 的 第 2 项 与 式 (5 - 25) 第 2 项 进行 比较 ,得 
[ P3. lexus = Cy Fun 7 Co(z, - Zu a) 
= Pina - Pina (Pina) dica 
(Pana - Pa. (Ps, Y) FRA] 
Pin- (Pha) (& Zuni) 
将 式 (5 - 27) 代 人 式 (5 - 26) ,求解 x, ,得 
fg, = Kasa + PR (PS, ) (8, — San) 
K, A P3, (Pa) 
现在 , 式 (5 - 28) 和 式 (5 - 26) 变 为 卡尔 曼 滤波 器 校正 方程 , 即 
Ban = < Eua) 
P - PO ~ K,P;,.K; 


(5 - 17) 


(5 - 18) 


(5 - 19) 


(5 - 20) 


(5 -21) 
(5 -22) 
(5 -23) 
(5 - 24) 
(5 - 25) 


(5 - 26) 


(5 -27) 


(5 - 28) 
(5 - 29) 


(5 - 30) 
(5 -31) 


这 里 用 实际 的 观测 值 z RE z, 3E B 2 PT, (P5, Panai = K, Pa, KL 这 
一 事实 .注意 在 推导 卡尔 曼 滤波 器 校正 方程 过 程 中 , 对 式 (5 - 1) 动态 方程 和 


式 (5 - 2) 观测 方程 的 线性 度 没 有 做 任何 假设 。 
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因此 ,卡尔 曼 滤波 校正 方程 可 由 高 斯 分 布 直接 推导 得 ,而 无 需 任何 假设 或 条 
件 。 然 而 , 记 住 这 一 点 很 重要 ,因为 这 些 校正 方程 比较 通用 ,可 以 应 用 于 任意 分 布 
的 前 两 阶 矩 ( 只 要 这 些 矩 存在 ) 。 必 须 指 出 ,到 目前 为 止 没 有 期 刊 文献 将 这 些 校 
正方 程 用 在 非 高 斯 密度 当中 。 


5.3 ” 贝 叶 斯 点 预测 积分 求解 高 斯 密度 


根据 动态 方程 (5 - 1) ,假定 ww 为 独立 于 x，e R" Az, e R": 的 零 均值 高 斯 
随机 变量 ,得 
Vaai — p(x, | Xna) = NOx, - [fi (x,.,) + u,];0,Q) = N_i 0,Q) 
(5 - 32) 
同样 ,对 于 观测 方程 (5 -2) ,假定 w, 是 一 个 独立 于 x, 和 z, 的 零 均 值 高 斯 随 
机 变量 ,有 
w, ~ p(z, | x,) = N(z, - h,(x,);0,R) = N(w,; 0,R) (5 -33) 
由 贝 叶 斯 法 则 ,得 先 验 密度 , 即 
P(X, | Zint) © p(z, | x,)p(x, | x,4) (5 - 34) 
这 里 假设 所 有 的 密度 函数 均 为 高 斯 分 布 , 令 P(xr, | za) 为 高 斯 分 布 ,因为 两 个 
高 斯 密度 的 积 同样 是 高 斯 分 布 , 则 有 
p(z,! Zini) = N(x,; Beni PS...) (5 = 35) 
pitaa l mua) =ë Ns Benes Pins) (5 - 36) 
假设 w 和 w, 为 零 均 值 高 斯 过 程 ,将 式 (5 - 36) 代入 式 (5 23) 和 式 (5 -4) ， 
式 (5 -35) 代入 式 (5 -5) ~ 式 (5 -7) ,预测 估计 过 程 可 以 用 高 斯 加 权 积 分 来 概 
括 , 即 


三 ANCA, À 
NU, $3, Poa. a Yx; , tu, (5 - 37) 
Print = [faai x) +u, ELA: 


EAE E -Xual- 


RE +Q (5 - 38) 


Ei = fae Gs -Pp Pus )dx, (5 - 39) 


Pia [Gh ELIGO, = k Z) ° 


N(x, 3 Kian) PS. i dz, +R (5 - 40) 
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Pa f. - Euni) (h,(x,) zi d . 


Ar, 5X, i Paa) aX, (5 - 41) 

R à |w,wiN(w,; 0,R)dw, (5 - 42) 
Rnx 

Q fri Nr; 0,Q)dr,., (5 - 43) 


R”: 


图 5 - 1 给 出 了 高 斯 概率 分 布下 一 般 贝 叶 斯 估计 的 流程 框图 , 包括 以 下 


步骤 : 


Wix, Rao PZ )dx,, 
Pes [D x,)—Z va LA, Zt Ja 
N (x: Xu, Pu, V dx, +R 

P =f [x. x, ] ^t, Ewa]: 
和 )dx, 
























卡尔 受 涉 波 器 : 校正 
K, Pa (Pia ) 

%, SX yt +K, (E, = ) 
NP EPS 2 








状态 向 量 预 测 
[T = f Erit N (x, , Xo a NIP )dx, 
Pay = Sau) = ]f aon. ) -Ea 
N na Enip- Pra )dx, HO 








Xn-ijr-1> Pai 





5-1 高 斯 概率 密度 贝 叶 斯 估计 流程 框图 


(1) 根据 初步 观测 ,生成 和 PP 的 最 佳 猜测 值 , 初 始 化 跟踪 滤波 器 ; 

(2) 计算 状态 矢量 预测 积分 ; 

(3) 计算 观测 值 预测 积分 ; 

(4) 执行 卡尔 曼 滤波 器 校正 方程 ,得 后 验 点 估计 这 和 P” ; 

(5) 将 结果 存储 在 跟踪 文件 中 ; 

(6) 时 间 上 前 进一步 ,利用 部, ， 和 PZ... 作为 新 的 输入 ,进入 下 一 轮渡 


波 器 迭代 ; 


(7) 返回 步骤 (2)。 
所 有 基于 高 斯 假设 的 贝 叶 斯 估计 方法 都 遵循 同样 的 基本 流程 ,只 是 在 函数 


通 近 或 者 积分 方法 上 有 所 改变 。 
5.3.1 利用 仿 射 变换 来 简化 流程 
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第 2. 4. 2.3 节 讨论 了 将 矢量 仿 射 变换 应 用 到 式 (2 - 106) 形式 的 一 般 高 斯 积 


分 当中 。 清 晰 起 见 , 这 里 再 重复 一 次 , 即 


e {f(x)! = ffo) : 


(Qa) | PFI? 


exp{ - > (= ~x)"[P*] “(x -x) bax 


= [fG0 NGOs, p=) de 
如 第 2 章 内 容 所 述 进行 矢量 仿 射 变换 

x = x + Dc 

则 得 
[fGoNGs x,P")dx = [f cote; 0 I) dc 

D 由 下 式 定 义 

P* = DD' 
或 者 ,可 写 为 

D = [p*]'? 


此 外 ,f (e) 将 定义 为 


f (c) À f(X + Dc) 
对 式 (5 - 37) 式 (5 - 38) 应 用 矢量 仿 射 变换 ,得 


uni = ff Ca) Nes0,Dde + u, 


Pia [[f (e) +u, ELA (e) +u, - Fyn)": 


N(c;0 I)dc + Q 


f (c) = fL + D, 4,46) 
D, 110-1 人 eet 
此 外 , 式 (5 - 39) ~ 式 (5 -41) 变 成 


Y = fh (e) Ce 50,1) de 
PS od = JER ce) -2na l[k (c) =E] . 
N(c;0,I)de +R 


= [UF (e) +x, -uni ][h(e) -ZJ Nes0,D)de 


(5 - 44) 
(5 - 45) 


(5 - 46) 


(5 -47) 


(5 - 48) 


(5 - 49) 


(5 - 50) 


(5 - 51) 


(5 - 52) 


(5 - 53) 
(5 - 54) 


(5 - 55) 


(5 - 56) 
(5 - 57) 
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h(c) = h (š... + D... ie) (5 - 58) 

Dy. = (PS...) (5 - 59) 

式 (5 - 51) ~ 式 (5 - 52) 和 式 (5 - 55) ~ 式 (5 - 57) 方程 组 需要 求解 三 个 如 
下 形式 的 一 般 矩 积分 , 即 


ICF) à [F (e) Ne;0,D)de (5 - 60) 


UF HF") a [LF (e) -1(f)]Lf (e) -ICF )]'ACei0,I)ae 
(5 -61) 


I(cf ) à [cf (c)NOc;0 I) dc (5 -62) 
5.3.2 求解 高 斯 加 权 积 分 的 一 般 方 法 


对 于 一 般 非 线性 函数 (ce ) ,无 法 求 出 式 (5 -60) ~ 式 (5 -62) 中 积分 的 解 
析 解 ,一 般 将 求解 这 些 积分 的 数值 方法 称 为 多 重 积分 规则 ,每 种 规则 可 以 求解 一 


类 f (c) EELMAA HRA ELR] 及 其 引用 的 参考 文献 中 , 均 采用 了 
类 似 方法 ,这 种 方法 通过 对 随机 变量 x 的 一 系列 变换 ,对 f(x) 进行 统计 线性 化 
处 理 , 然 后 将 变换 后 的 f(x) 进行 多 项 式 展开 ,计算 期 望 积分 。 

式 (5 - 46) 的 仿 射 变换 非常 重要 ,因为 它 将 一 般 高 斯 加 权 积 分 变换 为 完全 
对 称 。 密 度 加 权 积 分 完全 对 称 就 是 在 均等 轮廓 形成 的 超 球面 所 有 维度 上 ,密度 关 
于 零 对 称 . 此 外 ,在 所 有 维度 上 ,积分 的 上 下 限 也 必须 关于 零 对 称 。. 这 将 大 大 简化 
用 高 斯 加 权 积 分 对 非 线 性 消 数 进行 近似 的 数值 方法 。 

根据 解析 式 NM(c;0,1) 可 以 看 出 , 对 于 多 维 坐标 系统 下 的 矢量 点 ce, 
NGc”;0,7) 对 应 具体 值 。 对 于 n, 维 单位 半径 超 球面 上 任意 n, HE BUS c" ,可 
直接 得 出 Nc” ;0,1) = e'2⁄(2m) 以 上 还 可 以 推广 为 :对 于 一 个 给 定 半 
径 的 超 球面 ,对 于 超 球面 表面 上 的 任意 矢量 点 C Ne ;0,7) 取 值 相同 ,在 二 
维 的 情况 下 ,可 以 由 图 2 - 9 中 看 到 恒定 半径 Mec;0,7) 的 等 值 轮廓 圆 。 


假定 f (c) 是 线性 的 , 即 f (e) — Fe ,其 中 五 为 独立 于 c 的 确定 性 转移 矩阵 ， 
那么 式 (5 - 60) ~ 式 (5 - 62) 的 求解 会 非常 简单 , 式 (5 - 51) ~ 式 (5 - 57) 将 
简化 为 著名 的 线性 卡尔 曼 滤波 器 ,在 第 6 章 会 详细 介绍 。 


如 果 f (c) 为 非 线 性 ,常规 的 方法 是 将 f (c) 解析 展开 为 一 个 多 维 多 项 式 ， 


每 一 维 都 由 单项 式 的 乘积 构成 例如 ,对 于 ce = [ec ,c,]',f (c) 的 二 维 多 项 式 展 
开 为 
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m m 
f (e, ;C2 ) = > > ai, ic c? 
ñ 204-0 
= a(0,0) +a(1,0)e, +a(0,1)c +a(1,1)c,c; +a(2,0)c 
+ +++ + a(m,,m,)c}"' c7 (5 - 63) 


多 项 式 的 每 一 项 都 是 a(i i) e! c? 形式 的 单项 式 . 对 于 d 次 多 维 多 项 式 ,每 一 项 
fee Ar osx [T cj, uh i, > 0 R 2, i, Sd WF ERA, SRAM KRM 


Jd =m, +m, > > t 


对 于 非 线 性 函数 六 (ec) JU f (e) 的 积分 多 项 式 展 开 为 形 如 式 (5 - 60) ~ st 
(5 -62) 的 方程 ,得 一 般 多 重 积分 规则 。 下 面 介绍 积分 规则 5?] 的 p 精度, 对 于 式 
(5 - 60) (5 - 62) 形式 的 积分 , “如 果 一 个 规则 的 积分 单项 式 最 高 为 p 次 , 那 


AREE FUR p MEE BD ORE n [ [ ^i, 2 0 Y," ,5 大 P 但 对 一 些 次 数 
XY «p*18 BOUE" "7. 


多 项 式 展开 的 经 典 方法 是 将 f (c) 展开 为 泰勒 多 项 式 。 在 第 7 章 , 仅 保留 泰 
勒 多 项 式 的 线性 项 ,得 扩展 卡尔 曼 滤波 器 (EKF) 。 然 而 ,EKF 的 主要 缺陷 是 需要 
求解 雅 可 比 和 矩阵 ,对 于 高 非 线性 度 函 数 , 使 用 包含 二 阶 项 的 泰勒 多 项 式 可 以 得 更 
准确 的 解 ,但 它 需 要 额外 求解 Hessian 和 矩阵。 这 种 求解 方法 可 推导 出 二 阶 EKF, 
在 第 7 章 中 会 详细 介绍 。 

另 一 种 替代 方法 是 将 雅 可 比 和 Hessian 差分 矩阵 替换 为 多 维 中 心 有 限 差分 
近似 ,导出 FDKF,FDKF 是 第 8 章 的 主题 内 容 ,是 Sigma 点 卡尔 曼 滤波 器 的 第 一 


MAF Sigma 点 卡尔 曼 滤波 器 需要 求解 在 构成 多 维 网 格 的 Sigma AE f (Ce) 的 
值 . 此 外 ,对 于 Sigma 点 卡尔 曼 滤波 器 ,求解 方程 式 (5 - 60) ~ 式 (5 - 62) HE 
解 一 组 矩 方程 ,在 后 续 章 节 详 细 讨论 。 在 数值 积分 方法 的 文献 中 , 这 个 概念 方法 
被 推广 到 将 式 (5 - 60) 近似 为 如 下 形式 "" , 即 


n, 


Wf) = Y wf (c?),f (c) e R" (5 - 64) 


J=1 


式 中 : 权 值 w\Sigma 点 c” 和 点 的 数目 由 所 选取 的 积分 方法 决定 wj Ale” 的 


取 值 与 函数 f 无关。 如 果 积 分 域 的 维 数 为 1, 即 mw。 = 1, 这 种 近似 称 为 求 积 公式 。 
如 果 大 2, 这 种 近似 叫做 求 体积 公式 具体 可 分 为 :数论 方法 和 基于 多 项 式 的 方 
法 ,第 一 类 要 求 积 分 (Sigma) 点 在 多 维 网 格 上 均匀 分 布 ;第 二 类 方法 对 于 一 些 正 
交 多 项 式 集合 非常 精确 ,FDKF 属于 第 一 类 。 
下 面 要 使 用 一 个 超 球面 上 的 两 组 矢量 点 。 对 于 第 一 组 ,假定 矢量 点 ce'” = 
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q[1,0,…,0]” = gr? ,其 中 9 为 超 球面 的 半径 ,~ ”为 沿 某 一 坐标 轴 的 单位 矢量 。 
超 球面 上 其 它 沿 不 同 坐 标 轴 的 点 可 以 通过 对 第 一 个 点 改变 符号 和 /或 交换 维度 
来 得 ,例如 ,点 ce ”= g[ -1,0,…,0] 或 ”= 94[0,…,1,…,0] 都 和 初始 点 一 
样 ,在 同一 个 超 球 面 上 .实际 上 ,在 每 根 坐 标 轴 上 有 两 个 点 ,一 个 为 正 .一 个 为 负 ， 
这 样 一 共有 2n, 个 类 似 矢量 点 ,将 这 个 矢量 点 集合 定义 为 gr ,r 为 沿 n, 笛 卡 儿 坐 
标 轴 的 正 、 负 方向 的 单位 矢量 。 这 种 方法 得 出 了 UKF ,将 在 第 9 章 讨论 。 

对 称 性 也 可 以 用 超 球 面 上 等 角 间 距 ( 而 不 是 沿 着 坐标 轴 ) 的 矢量 点 集 来 表 
示 。 所 有 这 些 在 一 个 超 球面 上 矢量 点 将 形成 一 个 对 称 集合 ,从 原点 辐射 到 多 维 单 
形 的 顶点 。 如 果 给 定 一 个 点 以 及 点 之 间 的 旋转 角 ,那么 可 以 由 初始 点 得 集合 中 的 
所 有 点 。 可 以 看 到 ,这 个 集合 仅 包含 n, + 2 个 点 。 由 这 个 方法 得 出 SSKF ,在 第 10 
章 会 对 此 详 加 阐述 。 

实际 上 有 许多 这 样 的 点 集 可 以 作为 生成 器 。 比 如 ,在 一 个 方形 网 格 上 ( 而 不 
是 超 球 面 上 ) 的 两 个 矢量 点 c ”= 94[1,1,0,…,0] Me” = 94[1,1,1,…,1] ， 
每 个 都 可 以 作为 一 个 生成 器 ,生成 一 组 不 同 的 对 称 点 集 。 但 是 在 这 些 情况 下 ,所 
有 的 Me”;0,7) 值 并 不 一 定 都 相等 。 这 种 矢量 点 生成 器 将 在 第 11 章 中 介绍 ,还 
将 用 正 交 厄 密 特 多 项 式 得 出 CHKF。 


由 于 对 多 项 式 次 数 有 限制 ,对 f (e) 进行 多 项 式 逼 近 , 必须 满足 如 下 矩 方 
程 组 


> wf (e?) - [f.()NCG30,D de (5 - 65) 
stipif Ce) WH f (e) PETS IARI , KE i UTR, t 48 77 PR AR (8 HERR 
性 方程 组 可 以 用 来 求解 未 知 项 |w,c” | 3X (5 — 61) 的 矩阵 方程 变 成 


Y uf (e?) Fle?) = [FileFile)N(e0,Dde (5 - 66) 


j=l 


如 果 想 进一步 详细 了 解 这 些 数值 方法 ,可 参考 Davis 和 Rabinowitz[4] Evans 
fil Swartz?! 的 著作 以 及 其 它 参 考 材 料 。 
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递归 线性 卡尔 曼 滤波 器 (LKF) 由 卡尔 曼 "” 提出 ,在 文献 [3 - 5] 中 结合 
用 实例 进行 了 深入 讨论 ,这 些 书 的 新 版 本 或 原 书 的 再 版 现在 都 可 以 买 到 。 很 多 估 
计 问 题 动态 方程 或 观测 方程 是 线性 的 ,有 时 二 者 均 为 线性 。 因 此 ,这 里 将 单独 处 
理 动 态 方程 或 观测 方程 为 线性 的 情况 ,将 其 归 为 线性 卡尔 曼 滤 波 器 。 


6.1 线性 动态 模型 


对 于 线性 动态 模型 , 式 (5 - 1) 可 写成 
x, = Fx,, * Uu, + V. , (6 - 1) 
式 中 :F 为 独立 于 时 间 的 确定 性 转移 矩阵 。 多 数 情 况 下 ,不 需要 控制 因子 ,但 为 
确保 完整 ,后面 将 包含 这 部 分 内 容 。 
将 式 (6 - 1) 代入 式 (5 - 37) ,得 
Eos = F |æ, Nx disci Prin) OX, +B, (6 - 2) 
进一步 简化 为 
Suet = Fi usa T u, (6 3) 
同样 ,将 式 (6 - 1) 代 人 式 (5 - 38) ,得 
Ps... = JFUx a -fuaa llt ELLA 


NGA; T4 Hai F enini] + 
= F| |[x a Rey E EE TT 4l '« 
NUR aa oae Psi- )dx,_, | F' +Q (6 - 4) 
但 是 ,由 于 


Prisci = fix, a 7X8, alix. -Xaaal * 
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Axa r Sg. a Pin: a EE (6 =5) 
对 于 线性 动态 模型 ,有 
I PT. = FPZ T. F' + Q 
Q = sva via) (6 - 6) 
式 中 :2 为 动态 噪声 协 方差 矩阵 。 


6.2 ”线性 观测 模型 


如 果 观 测 方程 为 线性 ,可 将 观测 方程 写成 


z, = Hx, + w, (6 -7) 
XP IH An, x n, 维 , 且 与 x, 和 时 间 无 关 的 确定 性 转移 矩阵 -因此 有 
h(x,) = Hx, (6 -8) 
假定 观测 噪声 w, 为 零 均 值 过 程 , 则 式 (5 - 39) 和 式 (5 - 40) 可 简化 为 
Piaci = Hx, 4-1 (6 -9) 
Pi. = HPZ, H' +R 
R = e(w,w;) (6 - 10) 


式 中 :R 为 观测 噪声 协 方差 矩阵 。 
最 后 ,将 式 (6 77) 3X(6 -9) 代入 式 (5 - 57) ,得 


Piin- = fix, -Fani ][z, -i £3] Ai Esas Poi )dx, 
- fix, m X441 ]L Hx, * w, = HRaal š 
N(x, ; Za ae A 
= ftx. = x... I [t zu 
N(x, ; M Pu )dx, ILE + 
fts. = Kawsa ]wiN(x,; Faint ,P3,.,) dx, H' (6 = 11) 
s] He 
Piin- = Pu, H (6 - 12) 


6.3 线性 卡尔 曼 滤 波 器 


当 动 态 过 程 和 观测 过 程 均 为 线性 时 , 贝 叶 斯 估计 简化 为 LKF。 注 意 ,预测 方 

程 可 通过 先 验 密度 和 后 验 密度 的 一 般 分 布 ,由 式 (5 -3) ~ 式 (5 -7) 直接 得 , 因 

此 卡尔 曼 滤波 器 适用 于 各 种 密度 分 布 。 只 需要 用 到 对 动态 方程 和 观测 方程 的 线 

性 度 要求 。 但 是 ,密度 函数 必须 是 已 知 的 解析 形式 ,这 样 才能 采用 某 种 方法 计算 
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UB EVRA TE F ,这 已 经 被 证 明 是 一 个 必要 条 件 。 
表 6 - 1 给 出 了 完整 的 LKF WE. 
表 6 -1 线性 卡尔 曼 滤 波 过 程 


Ey 初始 化 和 各 和 PF 


nln-l = Fx, isa t+u,, 


Ph. = FP=,,,.,F" +Q 


Zajazi " Hå in-i ? 
步骤 (3) 观测 值 预测 Pini = HPH, H" +R 
PL = Př, H" 


pa o 


K, & PS. (PS. I y ° 
步骤 (4) 卡尔 曼 滤 波 器 更 新 CE PO lf Saa) 
F2. P Pm - K,Pz Ki 


将 名 ,和 PF EMI HE 
mmm) | — 





6.4 LKF 在 DIFAR 浮标 方位 估计 中 的 应 用 


LKF 可 以 应 用 在 单个 DIFAR 浮标 中 ,利用 包括 方位 角 和 方位 变化 率 在 内 的 
方位 观测 值 ,来 估计 状态 矢量 .观测 矢量 定义 如 下 
x, À [0,,0,]" (6 - 13) 
式 中 :方位 角 0, 为 浮标 原点 至 目标 船舶 的 直线 与 y 轴 正 向 (指向 正 北方 向 ) 之 间 
的 夹 角 ,规定 — 180° < 9 < 1805;0, 为 方位 角 变 化 率 。 对 于 简单 问题 ,不 需要 控制 
变量 u, ,动态 转移 方程 由 下 式 给 出 , 即 








x. = Fe, + v. (6 - 14) 
F = $ | (6 - 15) 
式 中 :v,_, 为 一 个 零 均值 高 斯 随机 动态 加 速 噪声 过 程 ,定义 为 mw ~ N(C0,0) ; 
pr 
0-q c (6 - 16) 
T-T 


Q 为 连续 噪声 过 程 的 动态 噪声 协 方差 ;4 为 方位 角 加 速度 品 声 的 方差 ,这 里 设 为 
0. 1。 连 续 噪 声 模型 下 Q 的 完整 推导 超出 了 本 书 的 范畴 , 感 兴趣 的 读者 可 以 在 文 
72 


献 [6] 中 找到 。 检 验 式 (6 -6) 的 状态 协 方差 预测 方程 ,得 9 的 单位 .注意 下 式 , 即 


2 2. 
To O oh Pa P” 
2 2 >. nln-1 


= FEP” e A F" + Q 


(6 -17) 








q 的 单位 可 推导 出 :由 于 47 的 单位 与 ;或 者 (速度 ) 的 单位 相同 ,那么 g 的 单位 
可 表示 为 v /i 或 者 a x to 同时 ,4 是 一 个 均值 约 为 零 的 标准 偏差 ,如 果 用 动态 噪 
声 来 代表 恒定 速度 下 的 路 径 偏离 ,标准 偏差 需要 考虑 正 、 负 偏差 两 种 情况 ,因此 
是 容许 偏差 的 两 倍 。 在 这 个 实例 中 ,允许 的 速度 偏差 为 V0. 1⁄2 = 0. 16rad/s, 
对 于 单个 DIFAR 浮标 ,输出 为 i, 时 刻 的 含 噪 方位 观测 值 。 线 性 观测 方程 可 
写 为 
z, = 0, = Hx, +w, 
e, ~ N(0,0;) (6 - 18) 
H - [1 0] (6 - 19) 
sO io, 是 方位 测量 的 标准 偏差 ,为 DIFAR 浮标 的 恒定 且 时 间 无 关 的 特征 。 
LKF 可 以 用 初始 化 为 
0, 
xu 
6, 


式 中 :和 为 DIFAR 的 第 一 个 方位 观测 值 ,可 任意 设置 为 如 = - 0. 09deg/s. 4-9] 
始 状态 协 方差 矩阵 为 
pr = |” pl (6 - 20) 
0 oj 

sth o; =- 0. O9deg/s;or, 为 观测 不 确定 性 ,单位 为 度 。 注 意 ,在 执行 LKF SERE 
中 ,方位 角 和 方位 变化 率 分 别 转换 为 弧度 和 弧度 / 秒 ,但 线性 实例 中 ,这 种 转变 
并 不 必要 。 设 DIFAR 浮标 的 输入 信号 信 品 比 为 204B, 时 宽带 宽 积 为 90 ,这 样 可 
确保 方位 观测 值 为 高 斯 分 布 。 

图 6 - 1 给 出 了 单 DIFAR 浮标 方位 跟踪 LKF 过 程 。 注 意 在 卡尔 曼 滤波 器 术 
正 步骤 ,计算 新 息 (有 时 也 称 作 测量 残 差 ) 的 公式 为 


0, = 0, - Ou, (6 —21) 
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图 6 -1 单 DIFAR 浮标 LKF 过程 框图 


有 时 ,观测 值 和 预测 值 正 好 落 在 — 180/180 不 连续 点 的 两 侧 , 这 称 为 相位 卷 裙 ， 
必须 特殊 处 理 , 否 则 跟踪 滤波 将 失败 。 最 简单 的 处 理 方式 是 计算 m, ,如 果 它 大 于 
180°, 则 将 Ôn n IRE 360°, ME m, AF - 180°, ôn 加 上 360°。 如 果 上 述 条 件 均 
为 假 ,不 执行 任何 操作 。 

图 6 -2 中 可 看 到 DIFAR 浮标 方位 估计 中 的 相位 卷 衬 问题 .如 图 所 示 ,滤波 
器 输出 的 估计 方位 用 黑 点 表示 ,上 面 释 加 的 灰色 线 是 从 浮标 到 目标 船舶 真实 方 
位 。 这 种 标 绘 方法 称 为 方位 时 间 记录 (BTR) 。 使 用 LKF 估计 , 选 定 滤波 器 更 新 率 
为 1s 方位 加 速度 噪声 为 0. 1 ,可 以 得 很 好 的 方位 估计 。 
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fit ic) 
6-2 LKF 方位 估计 与 单 DIFAR 浮标 真实 方位 的 比较 
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线性 化 卡尔 曼 滤 波 器 :扩展 卡尔 曼 滤 波 器 





对 于 适度 非 线 性 且 平 滑 可 微 函数 ,可 以 采用 解析 方法 将 非 线 性 方程 线性 化 ， 
这 样 线性 卡尔 曼 滤 波 器 就 可 以 用 来 处 理 非 线 性 动态 方程 和 观测 方程 ,为 实现 线 


性 化 , 非 线 性 函数 f (0). 可 以 展开 为 关于 平均 值 的 多 维 泰 勒 多 项 式 & = 0, 得 扩 
展 卡 尔 曼 滤波 器 02] 。 为 了 更 好 地 解释 这 种 方法 ,并 给 后 续 章 节 做 参考 , 首先 推 
导 标 量 扩展 卡 尔 曼 滤波 器 ( EKF) ,然后 将 其 推广 至 多 维 情况 。 


7.1 一 维 情 况 


对 于 一 维 情况 ,状态 预测 方程 式 (5 - 51) 和 式 (5 - 52) 变 成 
tun = [f CONG 0,1)de + u, (7-1) 


UE si = [LF Ce) + u, - Xan) (e; 0,1) dc +o, (7 -2) 


f (c) e YR, ua t+ Oa- ln-1C) (7 - 3) 
由 式 (5 - 55) 至 式 (5 - 57) 可 知 ,对 于 一 维 情 况 ,观测 预测 方程 变 成 
Žan- = fÈ CO Ne; 0,1)de *E 4) 
Pann = [A7 C) NC; 0,1)de - Figs ot, (7 -5) 
Print = Gus Jh Ce) Me; 0,1) de (7 -6) 
h (c) ~ hx ui + Oan) (7 - 7) 


7.1.1 一 维 状态 预测 


式 (2 - 38) 为 一 般 一 维 M 次 泰勒 多 项 式 。 对 于 三 阶 一 维 泰勒 多 项 式 f (c), 
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关于 零 平均 值 展开 , 则 式 (2 - 38) 变 为 
Fo) =F +[FFO] e+ zo]. 
*- 3 
"eas O] e (7 -8) 
代入 式 (7 - 1) ,得 
tn = f (0) [ANC 0,1)de + 


[< Sf Co) | [ewtes 0,1)de + 


BE ia Co] JJ exte: 0,1)dc + 


e c] JNC; 0,1)de + u, (7 -9) 
使 用 一 维 矩 方程 (2 - 115) 计算 积分 ,得 状态 矢量 的 一 维 预测 估计 为 
=F) [fO] +u, (7 - 10) 


注意 ,这 个 估计 中 <。 具 有 三 阶 精度 ,因为 式 (7 - 8) 中 的 三 次 项 的 积分 为 零 。 
7.1.2 一 维 状态 估计 误差 方差 预测 
仅 考虑 式 (7 - 2) 中 的 第 一 项 
We) = [UF (c) +u, ~ Rua I (e; 0,1)de (7 - 11) 
由 式 (7 - 1) ,可 进行 如 下 推导 , 即 
= [F cotes 0,1)dc + u, 
= [F (Nes 0,1)de + f (0) - f (0) +u, 
= F (0) + [LF Ce) - F (0) Ne; 0,1)de +u, (7 - 12) 
因此 式 (7 - 11) 变 成 
Ke) = f[t£ (o - £601 - [LF C) -Fo)]Nr;0,1)dr } NC; 0,1) de 
= JEF (e) - F (0) NC 0,1) de - 
{ [CF Ce) - F (0) Mes 0,1) de } | (T - 13) 
将 泰勒 多 项 式 (7 - 8) 保留 至 二 次 项 ,得 
LFC) - F(0)] = [2 JO] 2 +> fio] e é (7-14) 
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Fe) -FOF [£o] elio] o0] ° 


D 2 
rae 0] 7 (7 - 15) 


将 式 (7 - 14) XC. - 15) 代入 式 (7 - 13) ,利用 一 维和 矩 方程 式 (2 - 115) 计算 合 
成 积分 ,得 


I(c) = [$ Ao]. HS o] (7 - 16) 
因此 ,状态 误差 方差 可 写 为 
a = io. aso]. (7 - 17) 


由 式 (7 - 15) 可 以 看 出 ,估计 精度 可 达到 。 ghe KE. 
7.1.3 一 维 观测 预测 方程 
按照 和 上 面 两 节 相同 的 步骤 ,对 2。, 和 ous, 进行 推导 ,得 
Diw] (1 = 18) 
oan = [EE ] io]. (7 - 19) 
互相 关系 数 为 
OV asa = Frain [ch (e) NC 0,1)de 
= Gasinife{h (0) + [8 (c)] e+ 
iO] < e HAC 0,1)de 
"ETE (7 - 20) 
对 互相 关系 数 的 估计 可 达到 “ 的 3 WN. 


7.1.4 一 维 预测 方程 的 变换 
根据 仿 射 变 换 的 定义 (5 - 46) ,利用 微分 链 式 法 则 ,有 





d dx, 1 d 
T = a “sl xx,n-lln- 7 = 21 ) 
de de dx, “iol dy ( 

d 一 (2) d° 

de’ de ) dż 
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2 d 
xx,n-ll n-1 dx? , 


由 式 (7 -3) 8 f (0) =A nuni) HRS -46) 有 c 20— x, = 2, io 这 
样式 (7 - 10) 变 成 


(7 — 22) 


= uy 


= a) + Tola [1 fo]. "EL 
(7 -23) 
用 同样 方法 ,将 式 (7 - 17) 变 成 
eas = = + 
d eas ES f(x, at "m 
og, (7 - 24) 


对 于 观测 方程 ,利用 式 (5 - 46) 仿 射 变换 ,而 不 是 式 (7 - 21) \ 式 (7 - 22), 
链 式 法 则 变 成 


d dud. d 
de "T dc dx, = Ox, an-i dx, (7 - 25) 
d? d? 
dd = 94 ad (7 - 26) 
这 样 , 式 (7 - 18) ~ 式 (7 - 20) 变换 为 
2 
Aisa = h(x, a) * To [ E... )] » (7 x: 27) 
2 
WE dai = daia -h(x,) | A + 
l d? 2 
270 | Fahl) | +, : * 
ae (7 - 28) 
OL usi Oa alge h(x, | (7 - 29) 


7.1.5 一 维 线性 化 EKF 过 程 


上 面 提 出 的 EKF 中 ,包含 比 线性 次 数 高 的 项 。 标 准 线性 EKF 可 以 通过 截断 ， 
即 只 保留 线性 项 来 得 ,这 样 得 的 EKF 和 第 6 章 中 给 出 的 线性 卡尔 曼 滤波 器 具有 
相同 形式 。 一 维 线性 化 EKF 过 程 如 表 7 - 1 所 列 。 注 意 ,对 于 这 种 一 维 情况 ,有 
D... -1 三 Prmn=l .在 更 新 步骤 中 = Jy c, 时 刻 的 观测 值 。 
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表 7 -1 一 维 扩展 卡尔 曼 滤 波 器 过 程 


滤波 器 初始 化 初始 化 名 fl ol, 0 


— 














Sani = ft Nod *u, 





2 z- xm 
xx ni n-l = Oxy n-lin-l x 


d 2 
Ca eee 

















nem = hx, ua) 
2 
2 = d 2 
BRG) | ”观测 值 预测 lasa = Panes | GAC) ta teh 
^ n-l = Oren n- A as.) 
In -1 In-1 dx, Da 







— 2 
n 7 Fiii qa 






步骤 (4) 卡尔 曼 滤波 器 更 新 


^ ^ o ^ 
Fain = Xu nl + K, (zn ~ Sat ) 








2 = x 2 
Ox, nin 一 C xx nin-l = Ko}, nin-i 


| 存储 结果 | % š, olus 存储 到 航 迹 文件 中 
mm [CS 


7.2 多维 情况 











对 于 多 维 参数 的 多 维 函 数 ,f(c) 的 状态 预测 积分 由 式 (5 - 51) ~ 式 (5 - 
57) 给 出 ,由 式 (6 - 3) . 式 (7 - 23) 很 容易 看 出 ,在 线性 卡尔 曼 滤波 器 和 一 维 
EKF 中 ,确定 性 控制 方程 u, 对 于 状态 预测 值 是 加 性 的 ,在 其 他 滤波 方程 中 不 是 
这 样 的 ,所 以 ,后 面 将 忽略 u, ,如 果实 例 研究 需要 ,再 把 它 加 回去 。 


式 (2 - 63) 给 出 一 般 多 维 泰勒 多 项 式 , 关 于 c 的 零 均 值 ,进行 f (e) 的 二 次 
泰勒 多 项 式 展开 ,将 式 (2 - 69) 代入 式 (2 - 63) ,得 


f (e) = > Def (e) 


2 i 
— Y (c) ] (7 - 30) 
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因此 , 式 (7 - 30) 可 以 重 写 为 
f (e) = fF(0) + [BT 2] «x SO] c 


E 





9 p ?- 
ac ac Af (e) | «6 . 7 


7.2.1 状态 预测 方程 
由 式 (5 - 51) ,利用 泰勒 多 项 式 (7 - 31) ,可 以 将 预测 一 阶 矩 表示 为 
Ea = F (0) [N(e30,1)de + | > aJ Co] | [etes Dae + 


>| > af (e) | I fa N(c50 ,I) de + 





[XX Fey] [eeweeio,Dde (1-32) 
利用 矩 方程 (2 - 116) ~ 式 (2 - 121) , 式 (7 - 32) 可 简化 为 
a TS £ p I 
š. SFO + 2 [2 sah (o)] (7 - 33) 


如 果 式 (7 - 31) 中 包含 三 次 项 ,在 式 (7 - 32) 中 它们 的 和 将 为 零 , 因此 
式 (7 - 33) 对 于 三 次 是 有 效 的 。 


7.2.2 状态 协 方差 预测 方程 
由 式 (5 - 52) ,得 协 方差 预测 方程 为 
Pu [LF co - £... ][f (e) -£,,41'NCe;0,I)de + Q 
(7 — 34) 
注意 ,通过 加 减 (0), RG - 51) 可 重 写 为 
Ea =F (0) + [Lf C) -F0O)]Nei0,Dde (7-35) 
用 式 (7 - 35) PEE... ICT - 34) 变 为 
P,a = [UF C) - f (0))[f (e) - f (0) 'MCe;0 I) de - 


[JLF Ce) - f (0) Ce ;0,Dae | 


[f «o - f (0) 'wceso. Dac] + o (7 -36) 


重 写 式 (7 - 31) 的 二 次 泰勒 多 项 式 ,得 
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Fo -7 = [E dio] «203 s (grau 





[XA Fils (7 -37) 
利用 矩 方程 式 (2 - 116) ~ 式 (2 - 121) , 则 式 (7 - 36) 中 第 二 个 积分 项 变 成 
[LF Ce) -7(O]Nei0,Dde = 7| s Eo]. (7 - 38) 
同样 ,由 二 阶 泰勒 多 项 式 (7 - 31) ,可 以 有 
[f (e) - f (0))[f (e) - f (0) ]' 
= ([X i£], e, [x io]; 





[X : E e] ss] 





[X > aa O] K (7 - 39) 


逐 项 进行 外 积 运算 ,将 结果 代入 趟 (7 - 36) 的 第 一 个 积分 项 , 使 用 和 矩 方程 式 
(2 -116) ~ 式 (2 - 121) ,得 


[LF € - f 40) ILF Ce) - f (0) * NCe;0, D) de 
- [ijo]. [342 af Ce) | 
415 oF (e) L.X Era]. ] ,+ 
| : x vat ©), | > > a (7 - 40) 
式 (7 - 36) 则 变 为 
- [íi], [三 六 Fo] ,+ 
>| X £o L.X ZF") ] ,+ 
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Ee mat] [eh a. 
i=l ey ¿vg rudi izl fou i"*j Sali 

(7 - 41) 
如 果 保 留 所 有 项 ,状态 误差 协 方差 矩阵 一 直到 五 阶 均 有 效 ,因为 第 五 阶 项 的 积分 


为 零 。 
7.2.3 ”观测 预测 方程 


用 同样 的 方式 ,预测 估计 2,,., 和 可 表示 为 
Zuni = h(0) "zl > Lào]. (7 - 42) 








(7 - 43) 
fia Pri, 由 式 (5 - 57) 给 出 ,可 写 为 


PZ = D... |ch!(c)N(c;0.1)de (7 - 44) 
利用 式 (7 - 31) K h, (e) 展开 为 泰勒 多 项 式 , 则 式 (7 - 44) 可 表示 为 
P=, = D. ,h' (0) | eNe;0,1) de + 


m. [BF a ir dui 


*i 


[esie NC; " I) de (7 - 45) 
计算 积分 结果 
Pi, = D... [X x ^O], (7 - 46) 
注意 :结果 具有 三 阶 精度 ,因为 e 的 三 次 项 为 零 。 
7.2.4 多维 预测 方程 的 变换 


由 式 (2 -5) 矩阵 的 迹 的 定义 ,有 
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利用 矢量 链 式 法 则 

Wear eel % 
利用 式 (2 - 107) ,得 
Vx = V[r«Dc] =D" 
因此 , 式 (7 - 47) 变 成 
9 -trlpr V, VED) 
izi OC; 


由 于 迹 具 有 循环 置换 不 变性 , 式 (7 - 50) 可 重 写 为 


ny 2 
THE 


XG... 定义 为 Hessian 矩阵, 则 有 


- 

















D dei & W... YL. 
a u à a 
"Ps 0X, 4.1 0X, 1n, 
à ð 
Ox, i, OX, ,., Ox, tn, 


式 中 :9/9x,_1.; 表示 对 于 矢量 x, 的 第 ;个 分 量 求 偏 导 。 


^ 
*n-1 =Xn-lln—l 


(7 - 47) 


(7 - 48) 


(7 - 49) 


(7 - 50) 


(7 - 51) 


(7-52) 


对 于 状态 矢量 预测 ,由 式 (5 -46) 得 , 当 c = 0 时 x = x, ,, HX (5 - 60) 


得 f (0) = fC nuai) ,这 样式 (7 - 33) JSA 
Tisai = fusa) + 
Z tr|PZ0, Qaa MfG) d 
XE f(x, ,) 中 的 每 一 个 元 素 进 行 迹 | PH Grins | 运算 。 
遵循 同样 的 逻辑 , 式 (7 - 41) 可 变换 为 
Pani = Q + F PS eal asa + 


H tr} p LiB sus fx, ) ] š 


(7 - 53) 


[tr P7, 6G, alf (x,)] + HOT (7 - 54) 





| ne 4 [ V. JO] 


^ 
¥n-1 7X-1In-1 
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OX, 44 9, Mk 
= : E : (7 - 55) 
of, afa, 
Bx On, — M 
式 中 :HOT 代表 高 阶 交叉 项 干扰 。 
以 同样 的 方式 ,观测 值 预测 估计 Zu.) Poin HELP, 变 为 
£a = MES) + Str P, Ë... Ih(x,) E (7 — 56) 


F2 = R + A, PS VET, + 
Url PS Gani 1 h(z.)) - 


[tr{/P*_,G,,.,|a"(x,) ] + HOT (7 - 57) 








Pisi = ee NP. (7 E 58) 
óh, ah, 
OX, 1 Ox, 


Hy.) = [ W e D a . = 

















X, =*nin-1 i ! : 
ðh, ðh, 
Ox, , Ox, 5. ^ 
Xn 7X n- 
(7-59) 
Gh V. V 
ə? à ð 
ox? 1 OX, | x, n, 
= : e, : (7 - 60) 
ə ð a 
Ox Ox, | ox? x 


7.2.5 ”线性 化 多 维 扩展 卡尔 曼 滤 波 器 过 程 


上 面 推导 的 EKF 方程 包含 比 线性 更 高 次 的 项 ,标准 线性 EKF 可 以 通过 截 
断 , 即 只 保留 线性 项 来 得 ,因此 线性 化 EKF 和 第 6 章 中 给 出 的 线性 卡尔 曼 滤 波 器 
具有 相同 形式 。 
比较 表 6 -1 的 LKF 预测 方程 和 表 7 - 2 的 卡尔 曼 滤波 器 预测 方程 ,可 以 看 
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出 ,两 者 在 形式 上 唯一 的 区 别 是 ,EKF REHA LE Pana MA, fE 
更 新 步骤 ,z? 为 1, 时 刻 的 观测 值 。 
表 7 -2 £y RAS EHE 


滤波 器 初始 化 初始 化 各 和 PE 


= KG mi ) 
步 又 (2) 状态 矢量 预测 Pausa B US LUPO aa 
Py.a = B oes Oe asa +Q 


taxa = (Xp 9-1) 
Boa CLA Da... 
HEG) 观测 值 预 测 ` 
Pis = H, PS, EP +R 
PR = PZ HL. 
K, à PEL (Piim j^ 
步骤 (4) 卡尔 曼 滤波 器 更 新 CME S TT R 
n6 = FE = KP.) Ki 


存储 结果 Kr x, Al PX, 存储 到 跟踪 文件 中 


7.2.6 二 阶 扩展 卡尔 曼 滤 波 器 


有 了 时 , 强 非 线 性 度 导致 线性 化 EKF 无 法 收敛 ,在 这 种 情况 下 ,一 种 方法 是 使 
用 二 阶 扩展 卡尔 曼 滤波 器 (SOEKF) ^?! .SOEKF 预测 方程 包括 二 阶 项 ,因此 ,用 
二 阶 预测 方程 代替 表 7 - 2 中 线性 化 EKF 的 预测 方程 , 则 有 
Ens maa) + 


Fete | PEG f(x, a) (7 - 61) 


^ 
n=l * *n-1I n-1 















erm =Q + Pn P PS + 
2 Url Para asa Ae.) - 


CI MM OM- oua] (7 - 62) 
E ind = R(X 40-3) + a Ñ... | h(x,,) ^ (7 - 63) 
FI -R +Ë PE, HI, * 
l 


21 tri PS G... [h(x,)] ` 


86 


[tr] PZ nl n-1 G,,,-,{h"(x,)] (7 - 64) 
Prin = PL (7 - 65) 


x Jii HF (OE SC ER, DL os BE HE n] HEREA Hessian 矩阵。 


7.3 ”多维 协 方差 预测 方程 的 男 一 种 推导 


为 了 以 后 参考 ,这 里 给 出 EKF 状态 协 方差 预测 方程 的 另 一 种 推导 。 在 下 一 
章 ,将 用 这 些 推导 来 求解 有 限 差分 ,给 出 有 限 差分 法 和 无 迹 卡尔 曼 滤 波 器 之 间 的 
关系 。 除 有 限 差分 方法 之 外 ,无 迹 卡 尔 曼 滤波 是 用 来 计算 协 方差 积分 的 首选 数值 
积分 方法 。 


定义 矩阵 
g(e) A [f (e) -uill (e) - š$,,+T (7 - 66) 
则 式 (7 - 34) 可 重 写 为 
PS... = [g (ce)Ne;0,Dde + Q (7 - 67) 
上 式 有 与 式 (5 - 51) 形式 完全 相同 的 状态 预测 方程 ,得 
Pi. = 800) ++ [X š 240]. +Q (7 - 68) 


其 中 书 被 用 来 区 分 这 种 形式 与 式 (7 - 41) 。 
同样 ,观测 值 协 方差 预测 变 成 


Pi... = p00) +4[ > : 20], (7 - 69) 
p (c) à [h(e) - Z,alLh (e) -£,,4]7 (7 - 70) 
互 协 方差 矩阵 预测 方程 (5 - 41) 可 以 变换 成 
P“ = fF Ce) NCc;0 ID de T-A) 
r(c) À [f (e) -funi ][ h (ce) -£,,4]' (7 - 72) 
则 有 
Fi = 70) ++ [ X 2 iro]. (7 - 73) 


将 式 (7 - 66) 代入 式 (7 - 68) ,并 进行 微分 ， 将 其 结果 与 表 7 - -2 中 的 Pw,_, 进行 
比较 , 找 出 PZ, , 与 线性 化 EKF 协 方差 预测 估计 PT, 之 间 的 关系 , 即 


Pa. = Pia -ELS SFO], [3 55 (e)| | 0-70 
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可 以 看 出 ,由 于 P*,, 包含 SOEKF 部 分 高 阶 项 ,P*,，, 比 线性 化 EKF 得 的 PY 更 
为 准确 。 观 测 值 协 方差 预测 估计 可 以 得 同样 的 结果 。 

在 下 一 章 ,用 这 些 协 方差 矩阵 形式 来 推导 有 限 差分 滤波 器 (与 将 在 第 9 章 讨论 
的 无 迹 卡尔 曼 滤波 器 相同 ) , 因为 本 书 不 会 使 用 EKF 本 身 , 这 里 将 不 再 进一步 
推导 。 


7.4 EKF 在 DIFAR 船舶 跟踪 的 应 用 案例 分 析 


利用 EKF 跟踪 船舶 的 笛 卡 儿 位 置 和 速度 , 当 它 通过 一 个 浮标 场 时 ,建立 船舶 
关于 时 间 的 动力 学 动态 模型 ,以 及 方位 观测 值 与 船舶 位 置 之 间 的 关系 模型 ,还 需要 
讨论 两 个 问题 :如 何 初始 化 (x。 Pina) 以 及 如 何 选择 一 个 合适 的 加 速度 噪声 q. 


7.4.1 船舶 运动 动态 模型 


矢量 矩阵 形式 的 动态 模型 可 以 由 式 (4 - 1) 定义 的 船舶 状态 矢量 得 , 将 
式 (4 -2)、 式 (4 -3) RAFA 
x. = BX. * 9.4 (7 -75) 
这 里 增加 了 高 斯 白 噪 声 加 速度 噪声 项 mw- , 以 允许 船舶 跟踪 过 程 中 微小 的 变化 
和 机 动 操 纵 。. 将 时 间 不 变 转移 矩阵 F 定义 为 


1 T 0 O 
Fa 0 1 0 0 (7 -76) 
00 1 f 
0001 
并 给 出 
y? p? 
TT 0 0 
T 0 0 
Q À slv,v,| = q — (7 -77) 
T? 
€ D» = T 
7.4.2 DIFAR 浮标 观测 模型 
XE X. t, 时 刻 的 浮标 场 观测 矢量 为 
Z, = [0,,,7,044]- (7 - 78) 


每 个 浮标 的 观测 模型 由 式 (4 — 7) 定义 。 然 后 用 如 下 矢量 观测 方程 联合 所 有 浮 
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标 , 有 


z, = h(x,) + w, (7-79) 
Onna = h,(x,) *w,, (7 - 80) 
b x) = tan [= — ža) (7 -81) 
r, - Yn 
高 斯 白 噪声 过 程 观测 值 的 协 方差 w, 定义 为 
o 0 0 0 
0 o : 0 0 
Ràelww =|: : i: o] (7 - 82) 
0 0 oun 0 
0 0 - 0 cj 


式 中 :om 为 与 第 m 个 浮标 观测 值 相关 的 方差 ,在 模拟 仿真 中 ,假定 o, = 0 = 3°, V m. 
因为 动态 方程 是 线性 的 , 表 6 -1 中 的 LKF 状态 预测 方程 可 以 用 于 跟踪 滤波 
器 。 这 里 用 到 的 状态 预测 方程 来 自 于 表 6 - 1, 即 
Fani - FX, 5-1 (7 - 83) 
Prin = FPzaQaGF' + Q (7 — 84) 
然而 ,由 于 每 个 浮标 的 观测 方程 式 (7 -81) 明显 为 非 线性 ,必须 使 用 表 7 -2 
中 的 EKF 预测 方程 ,而 这 需要 计算 雅 可 比 矩 阵 吾 ,，,。 











雅 可 比 矩 阵 定义 为 
Bs = [ Vh (x,)]* x 

: X, 7X4 a] | 
ðr? 
0 

= [hy (æ) hy Qr.) ] 
ar, 
EA 
ðv? "ON 
ðh, ðh, 
ðr ov, 

=j j “° i (7 - 85) 
ðhy ðhy 
ðr, 9", Xn 7 Xn a-] 
展开 导数 项 ,得 
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nl n-i 
As. o 0 om (7 - 86) 
Tain- 7 % Fain-1 7 XM 
E iE 
0 ioi 0 
Rua = y 和 TE 十 (Eua (7 - 87) 


将 EKF 应 用 到 DIFAR 案例 分 析 时 , 所 使 用 的 观测 预测 方程 (来 自 于 
表 7 - 2) 为 


Ead = (Z...) (7 T 88) 
F2 x: M AM NR * R (7 =, 89) 
EL - s d (7 s 90) 


当然 ,这 里 使 用 表 7 - 2 步骤 (4) 中 标准 的 卡尔 曼 滤波 器 更 新 方程 。 
7.4.3 ”初始 化 卡尔 曼 滤 波 器 族 中 所 有 滤波 器 


在 后 面 章 节 , 将 对 卡尔 曼 滤波 器 类 中 所 有 跟踪 滤波 器 性 能 进行 比较 ,为 确保 
比较 的 合理 性 ,必须 对 所 有 的 跟踪 滤波 器 进行 同样 的 初始 化 ,首先 ,假定 目标 船 


舶 由 西南 朝 着 原点 径 向 人 站 ,如 图 7 - 1 所 示 , 最 初 的 距离 RR 为 4n mile, 
Y(3b) 





[rx(),. n0] 
目标 船舶 


图 7-1 初始 化 几何 示意 图 


给 定 一 组 初始 浮标 测量 值 19, ,m = 1, M| ,用 下 式 初 始 化 状态 矢量 位 置 
分 量 , 即 
ro = mean(r, - Rsin 0, ) (7 — 91) 
ry = mean(r" - Rcos 6,,) (7 - 92) 
SUP: (rnrn) A m TPA LE ; mean 为 所 有 浮标 的 平均 值 。 
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根据 速度 分 量 ,对 所 有 浮标 的 第 一 组 方位 测量 值 求 平 均 ,生成 目标 相对 于 原 
点 的 方位 估计 b = [ > On ]/M。 任 意 设 定 目标 船舶 的 速度 v = 30kn, 用 bo = 3 - 
T 计算 目标 航向 ,这 里 为 目标 航向 。 
初始 目标 速度 分 量 可 以 由 下 式 计算 , 即 
vy = v sind (7 - 93) 
vy = v cos 9 (7 - 94) 
基于 初始 化 过 程 ,可 以 发 现 ,如 果 想 由 第 一 组 DIFAR 元 素 方位 测量 值得 初始 方 
位 估计 ,必须 确定 两 个 变量 , 即 距离 R 和 速度 vo 
因为 没有 足够 的 信息 由 测量 值 协 方差 生成 初始 协 方差 矩阵 ,任意 指定 一 个 
对 角 和 矩阵 ,用 来 初始 化 状态 误差 协 方 差 ， 即 


P” = (7 - 95) 


o oc c 


9 
gw" 


mio = ay = 0.230%, = o 
7.4.4 选 定 加 速度 噪声 参数 


如 前 所 述 ,4 的 单位 为 At R a^ x to 对 于 像 船 舶 这 样 运动 缓慢 的 目标 ,使 用 
速度 的 变化 值 比较 方便 ;而 对 于 像 战斗 机 或 导弹 这 种 快速 运动 的 目标 ,使 用 加 速 
度 的 变化 值 (如 3g 转向 ,g 为 重力 加 速度 ) 更 为 适合 .对 于 缓慢 运动 的 船舶 目标 ， 
选择 9 = 10“ 以 允许 船舶 速度 由 于 波浪 作用 和 风力 引起 的 微小 偏差 。q 取 值 很 
小 ,是 假定 在 跟踪 间隔 中 ,没有 机 动 操纵 船舶 得 的 结果 。 对 于 一 个 机 动 操纵 的 船 
舶 , 取 较 高 的 g 值 更 为 合适 。 


7.4.5 EKF 跟踪 滤波 器 结果 


运行 EKF 跟踪 滤波 器 , 设 滤波 更 新 率 为 1s .船舶 接收 信号 集合 的 信 噪 比 为 
20dB , 时 间 带 宽 积 为 90 ,得 如 图 7 -2 所 示 的 跟踪 结果 。 这 个 航 迹 图 直观 地 给 出 了 
跟踪 算法 的 执行 情况 ,后 续 章 节 将 给 出 深入 的 性 能 测量 讨论 与 对 比如 图 所 示 ， 
由 于 初始 化 不 够 准确 ,跟踪 器 需要 一 段 时 间 才 能 收敛 于 真实 航 迹 ,但 收敛 于 真实 
航 迹 ( 用 灰色 表示 ) 的 速度 很 快 ,如 果 初 始 化 不 够 精确 ,跟踪 器 的 容错 就 非常 重 
要 ,因为 滤波 器 能 够 利用 观测 值 迅速 地 纠正 初始 化 错误 ,EKF( 以 及 后 面 所 有 的 
非 线性 卡尔 曼 滤 波 器 ) 与 初始 值 R 和 vw 的 选择 几乎 无 关 。 运 行 滤波 器 ,将 信 噪 比 
设 为 20dB ,距离 初始 值 为 20 ~ 3n mile ,速度 初始 化 为 10 ~ 30kn ,滤波 器 迅速 收 
敛 于 真实 航 迹 。 

在 第 4 章 中 给 出 , 当 信 噪 比 很 低 时 ,观测 值 的 方位 噪声 变 成 非 高 斯 分 布 的 。 
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Ww 
° 


Y 4g / 8 


45 sj -0.5 0.5 1 1.5 


0 
x 坐 标 / 码 
图 7 -2 通过 浮标 场 船 舶 的 估计 航 迹 与 真实 航 迹 对 比 


图 7 - 3 表明 输入 信号 信 品 比 对 高 斯 EKF 航 迹 估计 的 影响 , AMR LEA - 5 到 
20dB , 增 量 为 54B。 在 图 7 -3 中 ,黑色 线条 是 100 次 蒙特 卡 洛 运行 (采用 同样 的 初 
始 值 ) 得 的 滤波 器 跟踪 估计 ,灰色 线条 为 真实 航 迹 。 第 14 章 中 将 讨论 ,在 一 组 蒙 
特 卡 洛 运行 中 发 散 航 迹 比例 的 性 能 度量 。 在 图 7 - 3 中 列 出 每 种 信 噪 比 下 发 散 航 
迹 的 数目 ,由 于 有 100 次 蒙特 卡 洛 运 行 ,因此 发 散 航 迹 的 数目 与 发 散 航 迹 的 百 分 
比 相同 。 从 图 中 很 容易 看 到 , 当 信 品 比 小 于 等 于 5dB 时 ,由 于 非 高 斯 噪声 导致 的 
方位 观测 异常 值 过 多 ,EKF 跟踪 非常 困难 。 


+ 








4 
v 
Ë > SNR20:0 个 发 散 航 迹 Ü luas 信 噪 比 15; 0 4-2 icc 
= 8 
€ 0 4€ 0 
9S _ 
8 -2 A 2 
-4 
4 is i ós 0 05 1 15 2 -3 2 ai 0 1 2 
x 坐标 (n mile) x ABER (n mile) 
» 4 » 10 
= = 5 {AMR EE 5; 2 个 发 散 航 迹 
X i 
£ 0 D 0 
8 
$2 H -s 
"TT 0 1 2 3 wi 3.. oll 
x Ab AR (n mile) x Ab BR (n mile) 
o 100 e 
E 50 = 
x ° % 
a 
过 -50 E E 
—100 < 
-80 -60 -40 -20 0 20 40 -200 -100 0 100 20 30 


x 坐标 (n mile) x ALAR (n mile) 


图 7-3 6 种 信 噪 比 下 EKF 跟踪 器 输出 结果 比较 
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第 8 章 
Sigma 点 类 别 :有限 差 分 卡尔 曼 滤 波 器 





如 果 非 线性 程度 不 是 很 严重 ,线性 化 EKF 可 以 得 合理 的 估计 结果 。 对 于 动 
态 或 观测 转移 函数 为 高 度 非 线性 的 情况 ,可 以 在 EKF 中 包含 二 阶 项 ,来 实现 估 
计 。 由 于 需要 额外 计算 Hessian 矩阵 ,这 种 增加 的 复杂 性 有 时 很 难 实现 。 解 决 这 
个 问题 的 一 种 方法 是 用 等 价 的 有 限 差 分 取代 EKF 中 的 微分 运算 。 

Schei 首次 提出 , 在 非 线性 佑 计算 法 中 , 利用 中 心 有 限 差分 法 实现 线性 
1E? EKF 为 函数 f(x) 在 点 x 处 的 线性 化 , 则 Schei 方法 为 在 中 心 差分 点 + 
Dg 处 的 线性 化 ,由 于 Schei 只 取代 了 状态 (观测 ) 矢量 及 其 协 方差 预测 方程 中 的 
雅 可 比 和 矩阵, 所 以 这 种 方法 仅 在 状态 (观测 ) 矢量 预测 步骤 方面 比 EKF 更 为 
精确 。 

Norgaard 等 提出 一 种 更 为 精确 且 无 需求 导 的 非 线 性 系统 估计 方法 ,用 
t Stirling 插值 公式 代替 非 线 性 函数 的 泰勒 多 项 式 通 近 , 这 样 能 够 在 状态 ( 观 
W) 矢量 和 协 方差 预测 步骤 上 保持 高 阶 项 ,本 章 内 容 在 文献 [3 ] 基础 上 提出 。 

Ito .Xiong ^ 与 Wu 等 ”的 论文 推广 了 有 限 差分 允 近 方法 ,将 其 纳入 预测 技 
RIER ,采用 更 一 般 的 多 项 式 展开 方法 ,来 实现 非 线性 函数 的 通 近 。 

本 章 给 出 有 限 差分 卡尔 曼 滤波 器 的 完整 推导 ,从 标量 情况 开始 ,然后 推广 至 
多 维 情况 。 


8.1 一 维 有 限 差分 卡尔 曼 滤 波 器 
8.1.1 一 维 有 限 差 分 状态 预测 
式 (7 - 10) 给 出 了 f (c) 的 EKF 状态 预测 方程 标量 推导 ,为 清晰 起 见 , 这 里 


再 重复 一 次 。 
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~ 2 si 
Saa = £0) +y [57 eo] (8-1) 


如 前 所 述 ,c 具有 三 阶 精 度 。 
HRO - 47) 有 限 差 分 近似 法 替换 二 阶 导 数 , 则 (8 - 1) 变 为 


Kana = f (0) + Lf (9) -2f (0) +f (-q)] (8 -2) 
合并 相关 项 , 式 (8 - 2) 可 重 写 为 











? — ~ ~~ ~ 
¿. = [FVO Hho HOG  c-» 
q q 
d -1 
令 wy A 7 (8 - 4) 
q 
则 有 
1 
= 8-5 
q ers ( ) 
有 限 差分 计算 点 定义 为 
eñ as qr? (8 - 6) 
| 0 j=0 
OA 8-7 
i Dock j= 1,2 ( ) 


[1] e R' 符号 的 解释 可 参阅 第 2. 2 节 。 利 用 式 (8 -4) 和 式 (8 -6) , 式 (8 -3) 变 
为 


Ra = wo f) +S F (eo) + f(60)] (8-8) 


用 离散 值 取 代 式 (7 - 3) 中 定义 的 连续 c 值 ,得 


f (c9) Ë f s, uaa + Ou salar ) (8 -9) 
定义 一 维 有 限 差 分 Sigma 点 ,得 有 限 差分 计算 点 的 Xx, 项, 即 


tin j = 0 
^ 1 š 
Xn-iin-i 十 Fania J = 1 
^ 1 

lln-l 一 Oa n-iin-1 J = 2 


这 样 , 式 (8 - 8) 一 维 有 限 差分 状态 预测 方程 变 为 
2 = wo SX rin) + =T s + f i3 d (8 - 11) 


Fo, =o, A (1 -wo)/2, 式 (8 - 11) 可 以 写成 Sigma 点 的 和 , 即 
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2 
kici = 2, wf rn! ) (8 = 12) 


8.1.2 一 维 有 限 差 分 状态 预测 


用 式 (2 -44) 和 式 (2 -47) 中 的 有 限 差 分 等 效 代 替 式 (7 - 17) 的 一 阶 和 二 
阶 导数 ,状态 误差 方差 预测 方程 变 为 


Pana = ath 60 - FC Y + LF (a) - 27 (0) + FC 01 d, 
q 2q 


(8 - 13) 
利用 式 (8 - 4) (8 -6) 和 式 (8 - 10), (8 - 13) 可 简化 为 
1 — wo 


0 dai = 4 [Arat ) - fca ) ]° + 


L^ 0 š 1 0 
CM Sint) -An + 


+f )] + oo, (8 - 14) 
注意 ,方程 具有 5 阶 精度 ,这 在 第 7 章 已 说 明 。 


8.1.3 一 维 有 限 差 分 观测 预测 方程 
采用 与 前 面 两 节 同 样 的 步骤 ,给 出 一 维 观测 预测 和 互 方差 预测 方程 为 











2 
Žin- = 2 wih (Yel (8 = 15) 
j= 
l -— w 
Domisi ae, Dh rusa? 一 hall * 
(1 - w)? (1) (0) (2) 2 2 
— hun) 2h yu n-1 ) + hus) ] T Tas 
(8 - 16) 
9, uini = - ae Iwi[h Qr) 1) m A(x, )] | (8 i 17) 
Nu aci J = 0 
(p) PEP + 一 一 e a 1 J 
和 = a/1 一 Wo (8 - 18) 
x - l o? j = 2 
nl n=l xx,nl n-1 TE 
Jl -ws 


8.1.4 一 维 有 限 差分 卡尔 曼 滤 波 器 过 程 


式 (8 -12) 式 (8 -14) ~ 式 (8 -17) 构成 了 一 维 有 限 差 分 卡尔 曼 滤波 器 的 
预测 部 分 ,其 状态 和 观测 预测 具有 三 次 精度 , 协 方差 预测 具有 五 次 精度 。 表 8 - 1 
给 出 了 完整 的 一 维 有 限 差分 卡尔 曼 滤波 器 过 程 。 
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8.1.5 简化 的 一 维 有 限 差 分 预测 方程 


对 于 一 维 预 测 方程 可 以 进行 简化 ,最 简单 的 方法 是 取 w。= 0, 这 样 可 以 去 掉 
位 于 原点 的 点 ,得 没有 自由 参数 的 预测 方程 , 即 
表 8 -1 一 维 有 限 差分 卡尔 曼 滤 波 器 过 程 


设置 参数 0 < w < 1 
步骤 (1) 滤波 器 初始 化 


初始 化 和 xo Fil oo 
步骤 (2) 状态 矢量 预测 
步骤 (3) 观测 值 预测 
V sies = vid n-1 ° 
lw [hai - h(x- )]i 


p= Tiras Tot 
步骤 (4) 卡尔 曼 滤波 器 更 新 Sd. © haga + RO ~ Gua) 
ore - a, d a-1 = Ree mini 


存储 结果 将 二 ,和 PX, 和 存储 到 跟踪 文件 中 
| ems) | — GERERO | | 
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a 


X,-lln-1 j = 0 















U) = J*n-tin-1 + 
Xi EE T 1 -wo 


[f(x ai ) ii fa )]2 + 


É 二 2 
CT in) - f(y a) * 


Fata ) y ~ o, 
XA n-1 j =0 


^ l 
Xal n-l + ——— c; 


4. Jzl1 
xd. = 1 - wo mar j 


a 


1 , 
Sal 7 iT e j= 2 


yaa = ae wh ri, a) 


2 = l- Wo N 
Q> nl n-1 Mi 4 












-n 2 
ARa) - k 9.) +o. 





Che.) - 2M( 00) ) + hr 19]? «el, 













xun-i = iUt Un-1 ) tf ina) ] (8 - 19) 
id = Lir! Xil 1) - fai lln- $ Sh + 


Shah a) 一 2f (yt int) 
FOS ina) 1? + o. (8 - 20) 
E = UL) P ha ] (8 -21) 
"IE = 4 U^.) -h fedt ]? + 


XUL) = 2h) 


hall + os (8 - 22) 
I 
“APN - 29 xx,nl n al^. a) - hia] (8 -23) 
j Pitit T Oaai r= | 
X naa = f id aber. g : (8 - 24) 
Xn-lln-l 一 Cy. .a-lln-1 j = 2 
Stn + Fn dex [ug 
xac im J (8 - 25) 
Xn n-1 m O ix ni n-1 J = 2 


另 一 种 简化 方法 为 截断 高 阶 项 。 在 下 一 节 处 理 多 维 情况 时 ,将 更 详细 地 讨论 
这 种 方法 。 


8.2 多维 有 限 差分 卡尔 曼 滤 波 器 


8.2.1 多维 有 限 差 分 状态 预测 


与 一 维 情况 采取 相似 的 方式 ,首先 重复 一 下 式 (7 - 33) 给 出 的 EKF 多 维 状 
态 预 测 方程 , 即 


S41 =F) + y [Sho] (8 - 26) 


使 用 式 (2 - 74) 给 出 的 多 维 Stirling 多 项 式 的 二 阶 有 限 差分 项 , 令 x — c Ax, = 
0 , 则 式 (8 - 26) 变 为 


Bana = FO) +553 U Ge) -27(0) +F (-q6)1 
- 
= [£7] FO) «dz Y Uf Gn) £C) (8-2) 
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kb :e; 为 第 j 维 笛 卡 儿 坐 标 轴 方 向 的 单位 矢量 ;49 为 步 长 ,要求 必须 为 大 于 零 的 
有 限 实数 值 。 











定义 wo à 2 (8 - 28) 
q 
得 等 式 
] = 
J (8 - 29) 
q n, 
n. 
je eni (8 - 30) 


这 里 wo 为 一 自由 参数 ,决定 了 4 的 值 .为 保证 g 为 有 限 大 于 零 的 实数 ,必须 限制 
wo 的 取 值 范围 0 < o, < 1。 
这 样 , 式 (8 - 27) 可 写 为 


F404 = of (0) +- e y [/( OMA » e) 
(8 -31) 


为 进一步 简化 ,利用 2. 2 节 定 义 的 多 维 矢量 生成 器 函数 "” 。 例 如 ,在 四 维 的 
情况 下 ,有 








r? = [0,0,0,0]" 

r = [1,0,0,0]7 = e, 

r = [0,1,0,0]" = e, 

r? = [0,0,1,0]" = e, 

= 4r® = [0,0,0,1]7 = e, (8 - 32) 

r) 2[-1,0,0,0]" 2 - e, 

r® = [0, -1,0,0]" = —- e, 

r” = [0,0, -1,0]7 = —- e, 

r* = [0,0,0, -1]7 =-e, 
这 些 矢量 积分 点 形成 一 个 四 维 网 格 ,包含 沿 笛 卡 儿 坐 标 系 坐 标 轴 的 单位 矢量 和 
在 一 个 在 原点 的 点 。 如 果 排 除 位 于 原点 的 点 ,这 些 单位 矢量 都 落 在 半径 为 1 的 四 


维 超 球体 NM(c;0,7) 的 外 轮廓 上 。 二 维 图 形 化 示例 如 图 2 — 9 所 示 。 
对 于 一 般 的 n, 维 情况 ,有 





r= [Ul e R* J =0 (8 = 33) 
[1] e R” j = 1,--: ,2n, 
式 (2 -30) 给 出 [1] e R" 中 的 矢量 点 值 .离散 的 c 定义 为 

ce? = qr?) = ; P j = 0,++,2n, (8 - 34) 


则 式 (5 - 53) 可 以 写 为 
f (e) tie + ) (8 -35) 
ix B D, ,, , 由 式 (5 7-48) 定义 ,由 此 得 一 般 的 多 维 有 限 差 分 Sigma 点 的 定义 , 即 
Wii & X, naa * Deel” 


^ n . 
= X n-iln-1 + [To Dat} = O "=. 2n, (8 = 36) 


因此 , 式 (8 - 31) 可 以 重 写 为 


2n, 





Kasa = $ OF Ke 1-1) (8 - 37) 
j=0 
Wo j=0 
o= fiza TENE (8 -38) 
2n, EOS 


XH Y ^ o, = 1。 如 前 所 述 ,这 是 一 个 三 阶 逼 近 。 
8.2.2 多 维 有 限 差 分 状态 协 方差 预测 


式 (7 -41) 给 出 了 多 维 EKF 预测 状态 误差 协 方差 矩阵 ,为 清晰 起 见 , 这 里 再 
重复 一 次 。 








a 
T 
.| š š £7 (e) |. 
(8 - 39) 


对 于 (c) 的 每 一 维 ,利用 方程 (2 - 73) 和 式 (2 - 74) 给 出 的 多 维 Stirling 
多 项 式 的 一 阶 和 二 阶 有 限 差分 项 ， 式 (8 - 39) 变 为 


Pa. = ED EU Ge) - C ae UF Ge) - FC e) 1" + 


iy f (qe) - 2£ (0) + f (~ qe,) ] ` 
[f (ge) - 2f (0) + f (- qe) + 
ES Y S Ei +qe,) - f (qe; - qe;) - 


=l j=l k= 


f (— qe, + qe,) + f (- qe, - qe)] ` 
[f (qe, + qe) - f (qe, - qe) - 
f (— qe, + qe) + f (- qe, - qe) ]' + Q (8 - 40) 
代入 式 (8 - 28) 和 式 (8 - 36) , 式 (8 - 40) 变换 为 
Pies = Y Y UQ) FST 
Ufa) - fatio + + (1) - 


y X tat Ha-1) - 2f (xii ll a-1 ) +f GN 5 


[fx na) 7 Af Qi tnt) tft do i ]" + 

Q + HOT (8 -41) 
式 中 ;HOT 为 高 阶 交叉 项 的 总 和 。 
8.2.3 多 维 有 限 差 分 观测 预测 方程 


式 (7 - 42) 和 式 (7 - 43) 分 别 给 出 了 观测 值 和 观测 协 方差 -按照 上 节 使 用 
过 的 方法 ,用 有 限 差分 逼近 取代 微分 运算 ,得 





2a, »" 
- D wh Ont (8 - 42) 
] = 
= e axe. D - hil. 
n, j=l 


[hia - hus] + 





i(L- - wo) y [xy ) — 2h) + hae? )] 


[h(x t) - 2h(x a) +h(w022)]' +R (8 - 43) 


这 里 忽略 了 高 阶 交叉 项 , 且 
xD = ua + sss (8 - 44) 

由 式 (7 - 46) ,状态 观测 互 协 方差 预测 可 写 为 
P51 = Dual > ho] (8 - 45) 
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用 有 限 差分 等 效 替 换 偏 导 数 , 式 (8 - 45) 可 简化 为 
PS = Daoa a Y Lh (aa) = Btn) (8 - 46) 
代入 式 (8 - 36) 和 式 (8 - 38) , 式 (8 - 46) BH 


Pha |p, Y IG.) hos] (8-47) 


8.2.4 多 维 有 限 差分 卡尔 曼 滤 波 器 过 程 


上 面 推导 得 的 FDKF 方程 包含 比 线性 更 高 阶 的 项 , 因此 比 LKF 或 线性 化 
EKF 算法 更 为 准确 ,如 果 忽 略 HOT 项 ,FDKF 过 程 可 以 概括 为 表 8 - 2。 
像 对 一 维 情况 的 处 理 那 样 ,这 些 预测 方程 也 可 以 通过 设 wo = 0 进行 简化 ， 
这 样 可 以 去 掉 位 于 原点 的 点 ,得 没有 自由 参数 的 预测 方程 。 
表 8 -2 多 维 有 限 差分 卡尔 曼 滤 波 器 过 程 


10 <a@ <1 
步骤 (1 ) 滤波 器 初始 化 
初始 化 Xo 和 Py 


YOn E il + 
all. D, -lin 479 j = 0, 

Nas = Y wf y n- ) 

步骤 (2) 状态 矢量 预测 a! ll a-1 

- $0 
Fae i E ds 
Xe dore Utu. f OT.1- 
[fw far I" +Q 


了 和 





(hy?) - hye) JT +R 


SAG.) aat 





( 续 ) 


K, & Pq; (PS. I iy ' 
步骤 (4) 卡尔 曼 滤波 器 更 新 Xan = ¥,,, +K, (2 -Zi) 
Pie nla-l 一 K PaK 


= Ps 


8.3 多 维 有 限 差 分 协 方 差 预测 方程 的 另 一 种 推导 


应 用 有 限 差分 方程 (2 - 73) ,由 式 (7 - 69) 给 出 的 另 一 种 EKF 状态 误差 协 
方差 方程 变 为 


P=, = g (0) + aay Le (ae) -28(0) «g(-4e)] + Q 





= [£—"]g (0) + LY [g(qe) + 8 (- qe] +Q (8 - 48) 

q 2q £ 
由 式 (7 - 67) 得 g (c) E [f (c) -SaualLf (e) -X,,4) 。 利 用 与 上 面相 同 的 
方法 , 式 (8 - 48) 变换 为 


P. = Q + Y ufa. = E ] [Ie j = Beg not ]" 
(8 — 49) 
和 一 维 情 况 一 样 ,后 一 种 形式 的 预测 状态 协 方差 矩阵 只 是 二 阶 偏 导 。 后 续 章 


节 将 表明 式 (8 - 37) 与 式 (8 - 49) 对 于 无 迹 卡尔 曼 滤 波 器 来 说 是 相同 的 。 
注意 , 式 (8 - 43) 可 以 写成 另外 一 种 形式 , 即 





2n, 


Pu I = R + 2 wlha) - Zu all ha) -= Žanai] (8 — 50) 


此 外 , 式 (8 - 47) 可 重 写 为 
D: i - 0 = j ^ j ^ T 
P. > =a [Kyu sa) = X... a ][h( i) = Zuin- ] 
x j=0 


(8 — 51) 


因为 有 限 差 分 滤波 器 与 下 章 将 要 讨论 的 无 迹 卡 尔 曼 滤 波 器 几乎 完全 相同 ， 
本 书 仅 给 出 DIFAR 案例 分 析 中 无 迹 卡 尔 曼 滤波 器 的 处 理 结果 。 
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第 9 š 
Sigma 点 类 别 :无 迹 卡 尔 曼 滤 波 器 


9.1 单项 式 容 积 积分 法 


在 第 7 章 EKF 推导 过 程 中 ,所 有 的 非 线 性 函数 在 某 个 点 处 展开 为 泰勒 多 项 
x ,并 用 和 抑 方 程 来 计算 展开 式 中 各 个 单项 式 的 积分 。 这 构成 了 计算 高 斯 加 权 非 
线性 函数 积分 的 解析 方法 。FDKF 继续 采用 这 种 解析 方法 ,并 用 多 维 Stirling 插 
值 公 式 取代 泰勒 多 项 式 。Stirling 插值 允 近 要 求 计算 在 初始 泰勒 多 项 式 展开 点 
附近 各 维度 矢量 点 均匀 分 布 的 非 线 性 函数 。 尽 管 FDKF 和 Sigma 点 卡尔 曼 滤 
波 器 形式 相同 ,但 它 完全 用 解析 的 方法 得 ,因此 也 可 以 被 当 作 解析 线性 化 卡尔 曼 
滤波 器 类 中 的 一 种 。 

另 一 种 计算 高 斯 加 权 积 分 的 方法 是 利用 多 维 数值 积分 方法 进行 计算 。 在 一 
维 情况 时 ,将 这 些 方 法 归 类 为 著名 的 (对 于 了 解 这 类 算法 的 人 们 来 说 ) 正 交 积分 
方法 。 在 多 维 情况 时 ,将 它们 称 为 “多 维 正 交 ”或 “容积 ”积分 方法 02] 。 本 书 中 ， 
这 两 个 术语 可 以 互 换 使 用 。 | 

第 5 章 式 (5 -51) - 5k (5 -57) 的 高 斯 预测 方程 为 式 (5 -60) ~ 式 (5 - 62) 
的 一 般 积分 形式 。 本 章 将 给 出 一 些 容 积 方 法 来 实现 数值 积分 。 

对 于 一 般 多 维 高 斯 加 权 积 分 ,有 


B(x) = |8(e) N(c;0,1) de (9-1) 
由 第 5 章 可 知 ,多 维 高 斯 加 权 积分 可 以 用 式 (5 - 64) 的 离散 点 求 和 来 逼近 , 即 
a(x) = Y w ae”) (9 -2) 


这 里 wj、c” 和 nn, 的 具体 形式 取决 于 所 选 的 多 维 容 积 积 分 规则 。 一 般 来 说 ,n, 点 
规则 积分 计算 可 表示 为 
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R, = (we) j = e O 2, (9 -3) 
这 里 .为 积分 点 的 数量 ,通常 与 .有 一 定 关系 。 本 章 以 及 后 面 两 章 里 ,只 考虑 

单项 式 规则 ,下 面 将 给 出 其 定义 。 
给 定 矢量 c =[c ,c ,…,cu]r 和 一 个 多 维 d 次 单项 式 (参见 第 5 章 结尾 部 

分 ) 

aj, Joe e se (9 -4) 
EH eje +j ed. 例如 ,继续 第 5 章 结尾 处 的 二 维 例子 ,对 于 c e R, h 
d(ds2) 次 单项 式 扩展 成 的 多 项 式 基 为 集合 11,c c c ,csc d ET d 22, 
多 项 式 中 的 每 个 单项 式 必须 满足 ji ej <d 条 件 。 一 般 来 说 ,对 于 矢量 ce R° 有 
^( LIT 4 或 者 更 小 的 不 同 单项 式 。 二 项 式 系数 | ^ descr eoe 


况 下 ,从 nn 个 不 同 的 物体 中 取出 m 个 可 能 出 现 的 情况 数目 。 它 的 定义 为 


n n! 
ear ai Vies) 
89.1 2k-2Hd-2,EX 
f k+d\ (4) _ 41 _ g 
dim(k,d) 4 | : -hl aa- (9 -6) 


式 (9 -3) 为 n, ed KATREM, H D” wee”) HI #(e)w(e)de, 


g(c) 为 小 于 或 等 于 d 次 的 多 项 式 。 将 任意 单项 式 基 集 合 标记 为 16, ,b,,…， 
basa! ,用 法 则 R, = 1(9,, 07 ) ,j =1,…,n,| 对 每 个 次 数 小 于 或 等 于 d 的 多 项 
式 进行 积分 ,那么 必须 满足 密度 加 权 矩 方程 的 单项 式 系统 


Y ub?) = fb(e) Nes0,Dde i= 12.1, dim(k,a) 


=F} 
对 于 高 斯 加 权 密 度 , 式 (9 -7) 变 为 


Y. wb (e?) - [bi (e) wCe) de i 21,2,--,1,--,dim(k,d) (9-8) 
j=1 Rk 


fE 18 48 — 4" Ts J Ps E(B) 方程 的 单项 式 规则 ,由 于 dim 
(k,d) = dim(k,2) , 则 有 | b, „bz "inia » D aim(k,d) [一 | 1 ,CCC | ,进而 和 矩 方程 组 则 变 为 


Y v, z | NCe30,I) de = 1 (9 -9) 
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y we” = fe Ne;0,Ddc = 0 (9 -10) 


j=l Rt 


Y wee" = fec" N(e;0,I)de = I (9-11) 


j=l Rk 
PORMA — 2b E EAF k HER ER Bi LE REE 107 ,j = 1, 
2 | sa ,并 利用 它们 通过 式 (9 -9) ~ 式 (9 aa! ) 求 解 权 值 Wi; o 


9.2 无 迹 卡 尔 曼 滤 波 器 


9.2.1 产生 背景 


无 迹 卡尔 曼 滤 波 器 (UKF ) 最 初 是 由 1995 年 华盛顿 州 西雅图 美国 控制 会 议 
的 两 篇 论文 “中 首次 提出 。 这 两 篇 论文 提出 了 一 种 新 的 非 线 性 状态 估计 线性 
[E38 rJ ik, 5 EKF 不 同 , 它 不 需要 计算 雅 可 比 和 矩阵。 该 方法 能 有效 地 通过 高 
散 分 布 (矢量 点 ) 通 近 高 斯 分 布 ,允许 对 每 个 独立 的 点 应 用 非 线 性 转换 。 特别 之 
处 在 于 ,指定 一 组 关于 多 维 高 斯 分 布 均值 对 称 的 矢量 点 ,将 这 些 选中 的 点 集 输入 
到 非 线性 变换 中 ,利用 输出 结果 点 的 加 权 和 ,可 以 得 更 好 的 变换 后 均值 估计 , 优 
T EKF 得 的 估计 。 

在 文献 [6 -8] 中 ,Julier 详尽 地 图 述 了 他 的 算法 ;在 文献 [7] 中 ,他 将 这 种 方 
法 命名 为 “分 布 允 近 滤波 器 ”, 并 给 出 了 与 EKF 相 比 ,该 算法 在 处 理 高 度 非 线性 
函数 变换 中 的 优势 ;在 文献 [8] 中 ,Julier 提出 “与 任意 非 线 性 函数 的 通 近 或 转换 
HEEE ,概率 分 布 函 数 的 通 近 更 为 容易 。 然 而 ,在 文献 [9 | "P, Lefebvre 等 给 出 “对 
一 些 回归 点 ,进行 统计 线性 回归 ,通过 线性 化 过 程 和 测量 函数 ,得 出 完全 相同 的 
估计 ”。 上 述 方法 与 本 书后 面 即将 给 出 的 完全 解析 方法 有 一 些 相似 之 处 ,包括 
将 单项 式 积分 规则 用 于 非 线性 函数 的 多 项 式 和 逼 近 。 下 面 的 方法 不 涉及 任何 密度 
PRAGA UT ,而 是 对 于 选 定 的 积分 点 ,计算 高 斯 矩 方程 组 。 实 践 证 明 ,Julier 和 Uhl- 
mann 所 提出 的 Sigma 点 卡尔 曼 滤波 器 是 状态 矢量 仿 射 变换 的 必然 结果 ,得 的 估 
计 方 法 可 用 于 规范 化 高 斯 密度 的 简单 矩 积 分 。 

下 面 介绍 无 迹 卡 尔 曼 滤波 器 的 进一步 扩展 与 推广 。 参 考 文献 [10 ] 中 , Wan 
和 van der Merwe 扩展 了 UKF ,使 其 除 状态 估计 外 ,还 包括 参数 估计 。 文 献 [11 ] 
中 ,提出 了 一 种 平方 根 UKF ,可 进行 状态 估计 和 参数 估计 。 在 文献 [12 ] 中 ,将 
UKF 中 增加 额外 的 测试 点 ,使 得 UKF 可 应 用 于 不 连续 非 线 性 函数 。 在 文献 
[13] 中 ,Julier 推广 了 他 之 前 提出 的 UKF 算法 ,可 以 将 Sigma 点 缩放 至 任意 维 
数 。 在 文献 [14] 中 ,Tenne 和 Singh 将 UKF 推广 至 高 阶 。 针 对 那些 状态 (或 观 
测 ) 矢 量 可 以 被 解析 成 线性 和 非 线 性 分 量 的 情况 ,Briers 等 在 文献 [15 ] 中 提出 一 
种 Rao - Blackwell UKF 算法 。 最 近 ,Banani 和 Masnadi - Shiraz[ 16] 发表 了 一 种 
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迭代 UKF 算法 。 

在 van der Merwe[ 17 ] 以 及 Julier 和 Uhlmann[ 18 ] 几乎 同时 发 表 的 论文 中 ， 
对 最 初 的 UKF 以 及 其 诸多 扩展 算法 做 了 很 好 的 综述 ,文中 除了 给 出 一 些 实际 应 
用 以 及 示例 之 外 ,还 做 了 非常 全 面 的 回顾 。 


9.2.2 无 迹 卡 尔 曼 滤波 器 发 展 历程 


为 推导 无 迹 卡 尔 曼 滤波 器 ,假设 给 定 与 状态 矢量 维度 相同 的 矢量 点 |c”， 
j=1,2,…,1,…,n,| ,包括 构成 沿 着 每 根 笛 卡 儿 坐 标 轴 , 半 径 为 上 9 的 对 称 点 对 ， 
以 及 位 于 坐标 原点 的 单个 点 。 除 位 于 坐标 原点 的 点 ,所 有 矢量 点 都 分 布 在 同一 
个 n. 维 的 超 球面 上 。 两 个 矢量 点 在 每 一 维 中 的 对 称 性 ,保证 了 积分 点 的 总 数 为 
n,=2n, +1。 用 分 布 在 单位 超 球 面 (由 式 (2 -30) 得 ) 坐 标 轴 上 的 单位 矢量 集 r = 
[1] eR, 以 及 一 个 位 于 坐标 原点 的 点 ,将 矢量 点 集合 表示 为 e<”= gr”。 对 于 
n, 24,r 值 由 式 (8 -32) 给 出 。 

对 于 ce R',n, 24, fi n, 22n, +1=9 个 矢量 计算 点 。 利 用 式 (9 -9) ~ 
式 (9 -11) 给 出 的 矢量 点 ,得 如 下 和 矩 方程 组 , 即 


8 
$w =l (9 -12) 
j=0 

Š H 

Y, we? = qUw,r ? + wr +. + wer] = 0 (9 -13) 


8 
2 wee = lwr r" + err OT] Z 7 (9 -14) 
j= 


式 (9 -13) 可 重 写 为 


1 0 0 0 0 
0 1 0 0 0 
(to, — ws) 0 + (0, — w) 0 + (ws — w) 1 + (w, — ws) 0 = 0 
0 0 0 l 0 


这 可 以 通过 设置 o, = o, ,w 704,0, =w) 和 o, =w 来 实现 。 
式 (9-14) 可 写 为 


100 0 0 0 0 0 
(w, +ws)q " : ] : + (w, +w,.)q° M o ] : B 

000 0 0 0 0 0 

0 0 0 0 0 0 0 0 
cad 0 0 0 0 bae 0 0 0 0 

00 1 0 0.0 0 

0 0 0 0 00 0 1 
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1000 
_|o 100 "p 
0010 
0001 
进而 得 方程 组 
(m Fe hin Lng | (9 -16) 


(w, + 6 )q° = 2w,q = 1 —q = ra (9-17) 
因此 有 w = w,。 同 理 得 


w =w, j =2,3,.…,8 (9 -18) 
由 式 (9 -12) 可 知 
wo + 8w = 1 (9 -19) 
则 有 
wa. ^ (9 -20) 





将 式 (9 -20) 代 入 式 (9 - 16) ,得 


q = 于 (9 -21) 











可 以 推导 出 
c? - gr? = F = j - 0,1,:-.,8 (9 -22) 
Wo j=0 
w; = |: i (9 -23) 
8 J = 12, +3 
式 中 :wo 为 自由 参数 ,为 满足 式 (9 -12) 且 保证 所 有 的 权重 均 为 正 值 , 需 限制 0 三 
w <1, 


对 于 ceR“* 的 一 般 情 况 ,通过 数学 归纳 法 ,有 








9.2.3 无 迹 卡 尔 曼 滤波 器 状态 矢量 预测 方程 
由 式 (5 -51) \ 式 (9 -1) 和 式 (9 -2) ,可 将 状态 矢量 预测 方程 写 为 
š. = [FC N(e0.Dde = Yu, Fle”) (9 -26) 
利用 式 (5 -53) ,矢量 积分 点 ec” 可 写 为 
Fie”) = f(a + Bast) j =0,1,---,2n, (9 -27) 
将 UKF 中 Sigma 点 定义 为 
M aud À X, ua + Desai” j = 0,1,---,2n, (9 -28) 
式 (9 -26) 状 态 矢 量 预测 方程 则 变 为 


2n, 
$... = E S aar) (9 -29) 
j=0 


这 里 e?) 和 wj 分 别 由 式 (9 - 24) All st (9 - 25) 确定 ， AA D. iiss = 
[Piin] eo 


9.2.4 无 迹 卡 尔 曼 滤波 器 状态 矢量 协 方差 预测 方程 


人 
x 


g(c) A [fCe) - £,,4] (fe) - Fun)" (9 -30) 


将 其 代入 式 (9 - 26) 替换 fF (e), WSK (9 - 29) 变 为 如 下 的 状态 协 方差 预测 方 
程 , 即 


2n, 
Pd = fece) N (e50,I) de c Q = Y wg(c") +0 
j=0 


= NP — usa] : [fa a) ay NES E + Q 


(9 -31) 
9.2.5 无 迹 卡 尔 曼 滤波 器 观测 值 预测 方程 
利用 相同 的 方法 ,观测 值 预测 方程 推导 为 
Rass = Y wh(x ) (9 -32) 


2n, 
Plo = X wh.) = £451] [hr a) Ei] +R (9 -33) 
j=0 
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Pisa -Šu [fO tat) 7 X444] ° [À GT i) = 二] (9 -34) 


x. 1 PE £D c^ J = 0,1,:--,2n, (9 -35 ) 


注意 :UKF 状态 、 观 测 矢 量 预测 方程 式 (9 -29) 和 式 (9 -32) 分 别 与 有 
限 差分 预测 方程 式 (8 -37) 和 式 (8 -42) 的 形式 相同 ,而 UKF 协 方差 预测 
方程 式 (9 - 31) ,3X(9 -33) 和 式 (9 -34) 分 别 与 有 限 差 分 方程 的 第 二 种 形 
式 (8 -51) ~ 式 (8 -49) 相 同 。 


9.2.6 无 迹 卡 尔 曼 滤 波 器 过 程 


上 面 推导 得 的 UKF 方程 包含 比 线性 更 高 阶 的 项 ,因此 比 LKF 或 线性 化 
EKF 算法 更 为 准确 。 表 9 -1 给 出 了 UKF 过 程 的 总 结 。 
表 9-1 多 维 无 迹 卡 尔 曼 滤 波 过 程 


设 0<wo <l 
步骤 (1) 滤波 器 初始 化 


初始 化 和 2 和 p= 
Nisi =Z, w.< + Drinit j = 0,-.,2n, 
Xnln-1 = E Qu Fa af 


步骤 (2) 状态 矢量 预测 










Piisi - Y Fus) - $4.4) " Ufa =a.) > f + 
Q 


Xia = C +Dance” J = 0, ---,2n, 
步骤 (3) 观测 值 预测 


^ 2n. - i 
Zuln-l = X Sh Oba 
2n. ^ 
Pai = Y aw hn ~ £4 5-1] 
Ps. = XU gua) 72441 * EQ) -£,,]" 


[hQxdLi) - £441" + R 

K, A Pina (Pinia) 

PR, = Fe nad a 
[ewe Ch  . 


9.2.7 无 迹 卡尔 曼 滤 波 器 的 另 一 个 版 本 


从 式 (9 -12) 之 前 内 容 可 知 , 除 原 点 处 的 点 之 外 ,所 有 的 点 都 分 布 在 一 个 半 
径 为 q 的 超 球面 上 。 由 于 原点 处 的 点 可 以 任意 添加 , 取 o, =0, 从 积分 和 中 移 除 
位 于 原点 的 Sigma 点 ,不 会 损失 其 一 般 性 。 注 意 ,位 于 原点 处 的 Sigma 点 (位 于 
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r f) 5 €, ,, Evin Vis]. EXHALE Y UKF ,消除 了 选取 一 个 满足 0<wo <1 
条 件 的 值 的 任意 性 。 这 样 所 有 的 求 和 运算 具有 如 下 限制 :对 于 j=1,…,2n, ,有 
w=1/2n,; Vj, B y?) =£ + /n,Dr°? , 

在 文献 [2 ,20] 首 次 提出 球面 径 向 容积 准则 方法 之 后 ,Arasaratnam 和 Haykin[ 19 ] 
给 出 了 相同 的 结果 ,他 们 采用 球面 径 向 容积 准则 ,将 第 卡 儿 坐 标 下 的 高 斯 加 权 积 
分 变换 到 极 坐 标 下 。 在 推导 UKF 积分 准则 的 过 程 中 , 式 (9 - 12) ~ 式 (9 -14) 
需要 求解 2n, +1 个 联 立方 程 。 在 球面 径 向 法 中 ,利用 高 斯 加 权 积 分 的 完全 对 称 
性 质 , 将 联 立方 程 的 数量 减少 为 2 个 。 但 这 个 方法 与 上 面 推导 的 UKF 在 wo = 
时 的 结果 相同 ,都 是 非 线性 卡尔 曼 滤 波 器 。 


9.3 UKF 在 DIFAR 船舶 跟踪 的 应 用 案例 分 析 


由 于 在 DIFAR 跟踪 问题 中 ,动态 方程 为 线性 的 , 表 9 -2 给 出 了 解决 这 个 问 
题 所 采用 的 UKF 过 程 。 注 意 ,这 个 过 程 对 于 所 有 的 Sigma 点 卡尔 曼 滤波 器 通 
用 ,唯一 的 区 别 是 |(o,e”) yy=1,…,m,| 的 取 值 。 对 于 DIFAR 实例 ,在 表 9 -2 
中 ,n, =4, 取 r=[1] e R° ,加 上 位 于 原点 的 Sigma 点 ,UKF 中 共有 9 个 Sigma 点 。 
对 于 Sigma 点 ,| (w,,¢”) ,j=1,…,n,| 的 取 值 由 式 (9 -22) 和 式 (9 -23) 决 定 ， 
这 里 取 o, =0. 25。 

表 9-2 应 用 于 DIFAR 跟踪 的 多 维 Sigma 点 卡尔 曼 滤 波 器 过 程 


设 0 < Wo < 1 
步骤 (1) 滤波 器 初始 化 x 
初始 化 和 xo 和 PS 
Zaa = F £, usi 
步骤 (2) 状态 矢量 预测 Prin! = FPE, aap + Q 
Dan a ‘= [Pisa |” 
xy. I 2X. t D4,4c9 ij = 0,:--,2n, 
£a Eh) 
步骤 (3) 观测 值 预测 
卡尔 曼 滤波 器 P s 
步骤 (4) 更 新 Z... = X... y a. = Zua-1) 
a -K Pin KY 


Pon = Y wa) — Sunn) Ufa) - 24,417 eR 
FLA = RR 
存储 结果 | GR MPI, 存储 到 跟踪 文件 中 
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w [fru ) -Zas-1] b (hr) - £l y 













K, E Pai (Pini ) is 





在 这 个 UKF 滤波 器 中 ,采用 了 与 EKF 相同 的 初始 化 以 及 动态 噪声 值 9, 具 
体 参 见 7. 4.3 节 和 7.4.4 FA. 

在 DIFAR 实例 中 ,输入 6 种 不 同 信 品 比 的 信号 ,采用 UKF 跟踪 器 进行 跟 
踪 , 与 图 7 -3 给 出 的 EKF 跟踪 图 做 对 比 ,比较 100 次 蒙特 卡 洛 输出 轨迹 。 结 果 
KA, ZEA TAR LEAR TE P ,两 种 跟踪 算法 的 输出 结果 在 发 散 航 迹 的 数目 上 几乎 
相同 ,所 以 这 里 不 再 给 出 UKF 的 跟踪 结果 比较 图 。 
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第 10 = 
_ Sigmo 点 类 别 : 超 球面 单 形 卡尔 有 注 波 器 


对 于 UKF 算法 ,除了 在 原点 处 的 采样 点 ,和 矩 积分 方程 (9 -9) ~ 式 (9 -11) 
用 到 的 矢量 采样 点 关于 0 对 称 , 与 原点 距离 相等 , 落 在 属 维 笛 卡 儿 坐 标 系 的 坐 
标 轴 上 。 对 于 超 球面 单 形 卡 尔 曼 滤 波 器 ,要 求 关 于 原点 对 称 替 换 为 要 求 所 有 矢 
量 点 与 原点 及 彼此 间 的 距离 相等 ,因此 分 布 在 一 个 n, 维 单 形 ( 图 2 -4) 的 顶点 
上 。 这 一 采样 规则 由 Julier[ 1 ] 首 次 提出 。 

将 采样 (Sigma) 点 重新 定义 为 ce”” 一 ce,, 所 有 单 形 矢量 点 c, 都 必须 满足 矩 方 
程式 (9 -9) ~ 式 (9 -11)。 为 清晰 起 见 , 这 里 再 重复 一 下 , 即 


3 =, = 1 (10 -1) 
j=0 
Y wie, = 0 (10 -2) 
j=0 
i wee; = 1 (10 -3) 
J= 


10.1 一 维 超 球面 单 形 Sigma 点 


考虑 一 维 情况 (n. =1) ,在 一 条 直线 上 取 三 个 任意 点 (cn +C] ,C2 ) = (0,q, ; =q) , 
如 图 2 -4 Aran. HRE S ü A yB ES , 48 F Ià PH , BI 


wo +w, +w, = 1 (10-4) 
wq; — wq, = 0 (10 -5) 
wig, + wy = 1 (10 -6) 


注意 :矢量 点 必须 与 坐标 原点 等 距 。 令 q, =q ,由 式 (10 -5), 有 
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w, = w, (10 -7) 
由 式 (10 -4) ,得 











w, = w, = — (10 —8) 
根据 式 (10 -6) ,有 
l 
i= 10 -9 
q m ( ) 
因此 ,对 于 一 维 情况 ,Sigma 点 可 定义 为 
0 j=0 
c =lLLVzo j=l (10 -10) 
-1⁄ Viw j=2 
权 值 分 别 为 
0<w <1 (10 - 11) 
1 s 
Wy = (10 - 12) 
1 - 
w, = = (10 - 13) 
一 维 情况 下 有 x =x + oc, 得 一 维 Sigma 点 为 
Yo = š (10 - 14) 
Mi = £ + Gc. (10 -15) 
X: = K +o, (10 -16) 


10.2 ”二 维 超 球面 单 形 Sigma 点 


图 10 -1 给 出 了 二 维 情况 下 位 于 单 形 项 点 的 矢量 点 示例 。 在 这 个 等 边 三 角 
形 中 ,每 条 边 与 角 都 是 相等 的 ,所 有 的 采样 点 与 位 于 原点 的 点 距离 相等 。 在 二 维 
情况 下 ,矢量 采样 点 集合 为 (co,cl ,cz,cs) =1(0,0) ,(9q -93)",( 74, -0) ， 
(0,sq,) |, &#,s, 是 一 个 尺度 参数 ,其 值 将 在 后 面 求解 。 
再 次 强调 ,必须 满足 矩 方程 (10 - 1) 拢 方程 (10 -3) 。 由 式 (10 -1) ,有 
wo +w, +w, +w, = 1 (10 -17) 
由 式 (10 -2) ,得 
wq, -w,q, = 0 (10 -18) 
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c= (74,. -4)) 


c= (q,. -9,) 


图 10-1 由 4 个 矢量 采样 点 构成 的 二 维 单 形 
— 10 q, — wq + Was3ga = Ü (10 - 19) 
由 式 (10 -3) ,得 


0 ` 0 0 0 1 0 
Se a er cm 
2 2 0 sq, 0 1 


0 4 0 4 
可 简化 方程 组 为 
wiqi +w.q, = 1 (10 -21) 
wq, + wq, + wq = 1 (10 -22) 
由 式 (10 718) ,有 

uz = W (10 —23) 

且 式 (10 -22) 变 为 
2w,q; + 3s3g3 = | (10 -24) 


因为 每 一 个 点 与 原点 的 距离 相等 ,可 以 假定 每 个 点 的 权重 是 相同 的 w=w,。 由 
式 (10 -17), 有 








Wy = W; = W; = (10 —25) 
由 式 (10 — 19) 可知 s, =2, 这 样式 (10 -24) 可 写 为 
l l 
= = —— 10 -26 
和 addas 


综 上 ,在 二 维 情况 下 ,有 如 下 采样 点 
cz = [] » N (10 -27) 
0 0 
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š 
2D 
V2(2 + 1)w, 
| | 
2D 
€; = l 
V2(2 + 1)w, 
1D 
Co 
2D 
V2(2 + 1)w, 


权 值 为 


] — wo 





w: = 


j =1,2,3 


WW sk (10 -28) 和 式 (10 -29) 可 合并 为 一 个 方程 


1D 


] Jj = 1,2 


J/2(2 +1), 








10.3 高 维 超 球 面 单 形 Sigma 点 


对 于 一 般 的 n, 维 情况 , 则 有 


( 1)D 
€; 
e = | 1 | j = 1 …… n. 
Jn. (n, + 1)w, 


Sine 
e = | n. | 
Jn,(Cn, + 1)w, 
BUB 
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(10 -28) 


(10 -29) 


(10 -30) 


(10 -31) 


(10 -32) 


(10 -33) 


(10 -34) 


(10 -35) 


(10 -36) 





1 -wo . 
lai wi j21,-,n, +1 (10 -37) 
H T x =£ Dc EFE AMAR AE Sigma 点 , 即 
x" 2£DeP j-20,-,n,41 (10 -38) 


10.4 超 球面 单 形 卡尔 曼 滤 波 器 


式 (5 -51) 现 在 可 以 写 为 


n,*l 


É... = [fco A (c;0,I)de = PATAS CTET, (10 -39) 
由 式 (10 -38) ,可 以 确定 如 下 Sigma 点 , 即 

Wins = Kun. FO? jun, +1 (10 -40) 
Hp e? 由 式 (10 -34) ~ (10 -36) 定 义 。 运 用 与 UKF 类 似 方法 ,可 以 得 其 
余 的 预测 方程 。 其 中 ,状态 协 方差 为 


P2. = [8(e) NCesóD de = Y wg(c) 
j=0 


= Y went) se Bosa ] ` [fau )- a T + Q 


(10 -41) 
观测 值 预测 方程 为 

PEP = Y wh.) (10 -42) 
Pii Y wh.) ~ $4541] š (10 -43 ) 

jz0 
[hd a) -Zu.al] +R (10 - 44) 
PIA = > wlf rn) -Fiai ] d [AQ i) -= Zin] (10 -45 ) 
:$ = aoi PUT j20,-,n,41 (10 -46 ) 


10.5 超 球面 单 形 卡尔 曼 滤 波 器 过 程 


表 10 -1 概括 了 SSKF 过 程 ,注意 表 10 -1、 表 8 -2 和 表 9-1 之 间 的 唯一 
区 别 是 求 和 的 上 限 。 由 于 Sigma 点 卡尔 曼 滤波 器 之 间 的 相似 性 ,后 续 章 节 把 所 
有 的 Sigma 点 卡尔 曼 滤 波 器 合并 成 一 个 通用 的 过 程 。 
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表 10-1 多 维 超 球面 单 形 卡尔 曼 滤波 器 过 程 


设 0 < = Wp «1 
步骤 (1) | 滤波 器 初始 化 
初始 化 和 0 和 Py 


XS ucl = D, 1,407 j =0,- n, 
JUR) | 状态 矢量 预测 | Ban = Ys e at, a) 
P. ket 20 ‘wi FOP a) 754,421 * (FP a1) -X4,4]" + Q 


xdi = Xs 4 fD g e j*0,-,2n, 


-A = bs wih Ord i) 


观测 值 预测 "T " ^ 
Pj. = d SN wi Ard 1) 7 fasi] Ure — Zan] +R 


= = L1 u [f(x in 4) = Fu. 41 * [h(x ) -£,,4]" 


K, A Paat Pia) E 
TAa Sub dS : s 
步骤 (4) 更 新 Xar = Xain -| + K, (22 — Zan -1) 
Pa -1 = K. PS, Ka 


| O — 7 


10.6 SSKF 在 DIFAR 船舶 跟踪 的 应 用 案例 分 析 


因为 DIFAR 跟踪 问题 的 动态 方程 是 线性 的 ,求解 这 个 问题 的 SSKF 过 程 与 
表 9 -2 中 的 UKF 过 程 除了 求 和 的 上 限 , 其 余 均 相同 。 

对 于 DIFAR 情况 , 取 n,=4,c”" 一 ce”。 因 为 包含 了 位 于 原点 的 Sigma 点 ， 
所 以 SSKF 有 6 个 Sigma 点 。 对 于 Sigma Ñ, 1(0,, 67 ) ,j=0,1,…,5| 值 由 
式 (10 -34) ~ 式 (10 -37) 给 出 ,这 里 取 w。=0.25。 在 这 个 SSKF 滤波 器 中 , 采 
用 了 与 EKF 相同 的 初始 化 以 及 动态 噪声 值 g( 分 别 在 7.4.3 节 和 7.4.4 节 中 给 
出 ) 。 比 较 100 次 蒙特 卡 洛 输出 轨迹 ,对 于 不 同 信 噪 比 输入 信号 ,采用 SSKF ER 
踪 与 图 7 -3 给 出 的 EKF 跟踪 结果 是 相同 的 。 除 了 信 噪 比 为 5dB 时 发 散 轨 迹 的 
数目 (SSKF 为 3 个 ,而 EKF UKF 为 2 个 ) 略 有 差异 外 ,SSKF 5j EKF 和 UKF 在 
输出 上 没有 明显 差异 ,本 书后 续 章节 讨论 均 方 根 误差 时 ,将 会 给 出 进一步 证 明 。 
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一 维 高 斯 - 厄 米 特 方法 最 初 由 Wilf M OKB TE n BART) BB) A 
Ball 论文 ”中 提 及 的 那样 。 与 前 面 章节 提出 的 方法 不 同 , 高 斯 - 厄 米 特 积分 是 
一 种 正 交 方法 ,利用 正 交 多 项 式 代替 简单 的 多 项 式 。 

考虑 式 (5 -60) 中 的 一 般 多 维 积分 ,将 其 重 写 为 更 显 式 的 形式 , 即 


-C vt 


Kf) =a [heye (11-1) 


(20 y^ 


替换 变量 ,消去 指数 中 的 1/2 因子 , 令 





& — = —p”'(z = 3 11 -2 
a5 a (x - x) ( ) 
x = £ + /2Dq (11 -3) 
因此 , 式 (11 - 1) 变 为 
I(f) = VM 2 [Fae "dg (11 -4) 
f(q) S f(x + /2Dq) (11-5) 


11.1 一 维 高 斯 - 厄 米 特 求 积 
现在 的 目标 是 采用 某 种 方法 ,将 f(g) 展开 为 正 交 多 项 式 。 首 先 ,考虑 一 维 
情况 的 积分 


Pe [Fo)e7 dy (11 -6) 
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将 (D BAF A —ALIE EB IHS f(g) RTEUB FE 


fa) = 2, pib (a) (11 -7) 


这 里 1 中 (9) ,1=0,1,…,m| 代 表 一 组 正 交 多 项 式 ,多项式 b,(q)#l 中 (9) 与 权重 
函数 (或 核 函 数 )p(g) 在 区 间 [a,b] 上 正 交 , 即 


f pC) bi(9) 9. (da = e (11-8) 
这 里 c, 为 归 一 化 常量 ,而 


l k=l 
Ôr = | (11 -9) 
0 kzl 
对 于 式 (11 -6) 的 积分 ,可 以 使 用 厄 米 特 多 项 式 H, (q) , 权 值 函数 为 
p(q) = e* (11 -10) 
积分 区 间 为 [a,b] 一 [ — e ,% ] , X (11 -8) 正 交 条 件 则 变 为 
Je" HG G9 = Ad (11 -11) 
这 里 , 归 一 化 常量 由 下 式 给 出 , 即 
h, = VT224 (11 -12) 
可 以 利用 递归 关系 生成 厄 米 特 多 项 式 
H ,(q) = 0 (11 -13) 
H,(q) = 1 (11 -14) 
H, (q) = 2qH,(q) -21H, ,(qQ) 120 (11 -15) 
将 规范 化 的 厄 米 特 多 项 式 定 义 为 
H,(q) 
A, (q) 4 —— (11 -16) 
Jh 
进而 导出 正 交 条 件 
[ e BG. dg = 8, (11 -17) 
正 交 厄 米 特 多 项 式 的 递归 关系 为 
H (q) =0 (11 -18) 
Alq) = hj? = mw" (11 -19) 
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Bala) = a f eG -fraa 120 (01-20 
如 果 定 义 
B, 4 vi/ (11-21) 
那么 式 (11 - 20) 变 为 
q Aq) = BH... (q) +B, Hoa (q) (11 -22) 
将 矢量 h(g) e, VB HERE J, 定义 为 


h(q) A [E,() Hl (a) ED)! (11 -23) 
e, A [0,0,---,0,1]' (11 -24) 

0 B, `: 0 

B, 0 : 
J à (11 = 253 

H a B 

0 … Baa 0 


则 式 (11 -22) 的 正 交 厄 米 特 递 推 关 系 可 重 写 为 


qh(q) =J,h(q) +B, A, (qe, (11 -26) 
设 
H,(q) =0 (11 -27) 
则 式 (11 -26 ) 变 为 特征 值 方程 , 即 
[Jn - ql]h(q) 20 (11 -28) 


J, B m AS VAP AE ELH 1g, 80,1, m — 11 Ah, H f (4) B] m 阶 展开 式 的 插 
值 点 。f(9g) 在 不 同 的 特征 值 1g,,!1=0,1,…,m -1| 处 插值 误差 为 零 [2] ,由 于 
式 (11 -17) 正 交 条 件 中 包括 权 值 函 数 ,特征 值 可 以 写成 式 (11 -27) 归 一 化 解 的 
集合 , 即 





qi E l =0,1,---,m -1 (11 -29) 


这 里 p(g,) 由 式 (11 -10) 给 出 。 
考虑 m =3 时 三 阶 多 项 式 的 展开 。 尽 管 高 阶 多 项 式 展开 方法 是 由 一 维 情况 
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推广 而 来 (如 文献 [4,5] ) ,但 是 当 将 高 阶 方法 应 用 到 多 维 情况 时 ,很 快 就 变 得 难 
以 控制 。 对 于 m =3 , 式 (11 -25) 变 为 











0 B 0 0 1⁄2 0 
J; | 0 Bj, |= 1⁄2 0 l (11 -30) 
0 Bp, 0 0 1 0 
式 (11 -30) 的 特征 值 由 如 下 集合 构成 , 即 
19 ,9 ,9 = 31. 1,9,11 (11 =31) 
^ r&10,1, 21] (11-31) n 3858 Jg 
14,54, ,4,1 = Gr (11 -32) 
函数 /9) 现 在 可 以 用 规范 化 厄 米 特 多 项 式 的 三 阶 展开 式 逼 近 , 即 
2 
f(g) = > HG) (11 -33) 


由 于 f(g) 在 特征 值 点 的 展开 是 准确 的 ,将 每 个 特征 点 代入 式 (11 -33) ,得 矩 方 
程 组 为 


F(a) = Y dla) (11 - 34) 
Fa) = > afla) (11 - 35) 
f(g,) = > a0.) (11 -36) 


这 三 个 方程 有 三 个 未 知 数 | a, ,a; ,os | ,未 知 系数 是 可 解 的 。 将 矢量 f a URE 


[c AE SUN 
f a [f(q.),f(q.).f(q,)]' (11 -37) 
a S[a,a,,a,]° (11 -38) 


H,(q,) H, (q,) A, (q,) 
A a| Aq) Ala) Ala) (11 -39) 

Ha) Ha) #H.(q,) 

方程 组 (11 -34) ~ 式 (11 -36 ) 可 重 写 矢 量 矩 阵 方程 为 
f = Po 一 a = H''f (11 -40) 


将 式 (11 -12) ~ 式 (11 -16) 代 入 式 (11 -32) ,得 
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H 的 逆 为 





T^ I B J2 
1-8 42 
2/3 1/6 1/6 
H'- 2 0 $245 <= 125 s 
- 2⁄3 13⁄2 1⁄3 ⁄2 


将 式 (11 -42) 代 入 式 (11 -40) ,得 


EE MEE d -33) 变 成 


为 避免 混 消 ,在 多 维 情况 时 ,将 一 维 插值 点 集合 及 其 权 值 定义 为 


ao 


a, 


ao = 


a, 


a, 





2 -42/3 


求解 式 (11 - 43),181a,,a,,a,| 为 


vt 


m ^ 





2/3 


LN 
= 17/2 


f(0) 


1/6 


l > 


三 /0) EN 


1/6 
1/2/3 -1/2 s I($) 
1/3 /2 


1/3 ⁄2 N=) 


2-45), 
p 
mE /| 


fp» = aoh (q) 


f(q) = $40) + 67( /二 +67(- /3) 


tg qq) = {o.,/2, - /| 


= {2/3,1/6,1/6| 
这 样 , 式 (11 -48) 的 厄 米 特 多 项 展开 式 变 为 


| Po Pi P2 | 


f(q) = 


2 
>. fq”) 


(11 -41) 


(11 -42) 


(11 - 43) 


(11 - 44) 


(11 -45) 


(11 -46) 


(11 -47) 


(11 —48) 


(11 -49 ) 


(11 —50) 


(11 -51) 
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Rg"? p, 分 别 由 式 (11 -49) 和 式 (11 -50) 给 出 。 
11.2 一 维 高 斯 — JE 2K S FAR SE I IR ER 
一 维 情况 下 , 式 (11 -4) 变 为 
WF) = /iWe ds 


TIT "(a= x) 
B s 


x = £ + /2oq 


将 式 (11 -49) 代 入 式 (11 -54) ,得 一 维 高 斯 厄 米 特 Sigma 点 为 


x? = š+ /2oq' 
此 外 ,有 


Fla’) = f(x + Boq”) = f) 


则 式 (11 -52) 变 为 


z DM 1 f -8 - : i) 
IQ = X. pfa ) =) dq = Xp fa) 


s: > pfo | N(ex0,1) de 
由 于 密度 函数 的 积分 等 于 1, 式 (11 -57) 可 简化 为 
Kf) = Y pf) 


利用 式 (11 - 58 ) ,一 维 状态 预测 方程 (5 751) 可 写 为 


Sat n=! = > pif Oui) 


(i) 


° = wa (i) 
X,_ua-1 - X5-I n-1 * "T PET 


0 ¿ = 0 
r4[l]eR' = (1 š =l 
-1 i=2 


对 于 一 维 状态 变量 预测 , 令 


glc) À [f(c) - £,,4]? 
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(11 -52) 


(11 -53) 


(11 -54) 


(11 -55) 


(11 -56) 


l -2N 
F Í e dc 


(11 -57) 


(11 -58) 


(11 -59) 


(11 -60) 


(11 -61) 


(11 -62) 


则 一 维 状态 变量 预测 方程 (7 -2) 变 成 


om-l = face) N (e50,1)de o7, (11 -63) 
注意 : 式 (11 -63) 中 的 积分 项 与 式 (11 -6) 相 同 ,所 以 有 
Lact = Xp FOS ue) Soil + Onn (11 -64) 
用 同样 的 方式 ,将 式 (5 -51) ~ IX(5 -52) 简 化 为 
ii = paw, (11 -65) 
Tanini = X dei) — Sasa P + Pay (11 -66) 


2 f | : 
US dac =< > PiLf isi) — £un-1] [hs a) Kanal (11 -67) 


355.1 = A aud * Bou aa ir (11 - 68) 


式 (11 -59) 和 式 (11 - 64) ~ 式 (11 -67) 构 成 了 一 维 高 斯 - 厄 米 特 卡尔 曼 
滤波 器 预测 方程 组 。 这 里 没有 给 出 一 维 情况 的 过 程 流 图 ,不 过 可 以 把 它 作 为 下 
节 多 维 情况 的 一 个 特例 。 


11.3 多 维 高 斯 - 厄 米 特 卡 尔 曼 滤 波 器 


如 果 要 先 构建 插值 多 维 多 项 式 ,那么 构建 多 维 正 交 多 项 式 积分 法 则 是 一 个 
很 难 实现 的 任务 。 构 建 多 维 容积 规则 的 最 简单 方法 是 由 一 维 积分 法 则 构建 乘积 
法 则 。 首 先 考 虑 二 维 情况 ,4 = [a ,9 】 ,将 f(g) 展开 为 二 重 累 加 和 , 即 


32-1 


2 2 f | n" 
f(q) = XL Para h(i" ar) = Yu, fq") (11 -69) 


式 中 :pi 和 pi 由 式 (11 -50) X ;qi" Ag,” 由 式 (11 -49) 定 义 。 式 (11 -69) 
的 双重 累加 和 可 以 变换 成 索引 为 |j =0,1,…,8| 的 单 重 累加 和 , 权 值 变 为 


« -(2) =n (11 -70) 


l i 
w, =w, =w, =w, =( 3) (5) =Po: =pm i=1,2 (11 -71) 
l 2 
ws =w =w, =m = (6) =pp=pp il=1,2 (11 -72) 


将 式 (11 -70) ~ 式 (11 -72) 可 写 为 更 紧凑 的 形式 , 即 
127 


v A (+) (Z) j= 1,…,4 (11 -73) 


由 式 (11 -50) ,得 


g” = [Sr (11 -74) 


[0] e R° = [0,0]" j=0 
[1,0]" J = ] 
[1] e R? = [0,1]' f = 2 
[-1,0]* J = 
r’ = fo, -11* j=4 (11 -75) 
hat J = 5 
[rl em ede ltl .T= 
[1, in 1]? 4 i 7 
[-1, -1]' J = 
对 于 4 维 状态 矢量 采用 同样 的 方式 ,jg ) 展 开 式 可 写 为 
Fa) = Y X, Y Yapa (a ai? ai a) 
= xx f(q? ) (11 -76) 
采用 与 推导 式 (11 -73) 相 同 的 的 乘积 组 合 方式 ， 得 权 值 集合 
(3) i= 
(H 了 = 
j = 9,…,32 (11 -77) 


w 


— — 
= el al 
~~ — 

~ 

II 

Ww 

Ww 

z 


- 


.8 
[i 
— — — — 
中 | 一 wjn wjn wjn w | 


meee ee M 


由 式 (11 -50) ,得 
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g” = 7o (11 -78) 


[0] e R° j20 
[1] e R° j= 1,-,8 
r = 4[1,1] e R° j = 9,…,32 (11 -79) 
[1,1,1] e R* j233,--,64 
[1,1,1,1] e Rê j = 65,…,80 
这 可 以 推广 至 一 般 的 n, 维 情况 ,有 
3"x-1 
f(q) = 2, vw, fla”) (11 -80) 
这 里 g” 由 式 (11 -78) 给 出 , 且 有 
[0] e R" j20 
[1] e R" j = L =>. 98 
[1,1] e R" j22n41,-,2(n +n”) 
[1,1,1] e R” j=[2(n+n? «1],--, 


[2(n * n? ) + 23] 
GV) = ° 
n-2 . 
[1,1 1 eR" j-2[2in*n?") + XZ 2nj] 
+ [1,--,2"7,,,,] 
[1,1,1,,1] e R" j= [2(n +n?) + Y 2n] 
+ [1,777,2"n, ] 
(11 -81) 


gy fiie UL , FH] n 代替 n, ,并 分 别 用 式 (2 724) IX (2 725) 8E n" 和 n BEATE 
X. TRKE, Antl 个 重复 。 权 值 可 表示 为 


(F ise 
PRERE 
Y" jme stem aceon) 
-— : " (11 -82) 
GJ ie eem e xd 


+ [1,52 ay] 
(4) j = [2(n « n? ) + To fta] 
+ [1,--.,2"ne, ] 
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现在 ,将 式 (11 -80) 给 出 的 jg) 的 一 般 厄 米 特 多 项 展开 式 代 入 和 矩 积 分 方 
程 (11 -4) ,得 


u Z 3nx 一 1 4 
UP) = quy] ym Fa? yeaa (11 -83) 
将 变量 变 回 e 的 积分 , 式 (11 -83) 变 成 
3^.-1 3".-1 
Kf) = Yew fla?) | NG30,Dde = Y w f(g?) (11-84 


利用 式 (11 -5) 变 换 f(q? ) , 则 式 (11 84) 变 成 


3^,-] 
I = Y w,f(X + J3Dr?) (11 -85) 
j=0 
4 
Weta 4 Ble + JD. ,,, .,r (11 - 86) 


利用 式 (11 -85 ) 计 算式 (5 -51) ~ 式 (5 -52) ,得 一 般 高 斯 - 厄 米 特 状 态 预测 方 
程 为 


3nx-1 
Ñu. pe > ;fre ) (11 -87) 
j= 


Pa wtf Pant) 一 Nan-i]。 [FP int) Ka] + Q 


(11 - 88) 
同样 ,定义 | 
x2, i š. * BD, ur? (11 -89) 
观测 值 预测 方程 式 (5 -55) ~ 式 (5 -57) 可 写 为 
3^.-] 
Sen = Y wh xe) (11 -90) 
j=0 
3^,-] 
Pisi = 2, LACH) — Far] ° (11 -91) 
(A( Xd.) PME (11 -92) 
3n. -] 
PI >, w focis) Bal ° (11 -93) 
[Ex a) = 2,041" (11 -94) 


注意 :高 斯 - 厄 米 特 预测 方程 与 UKF 和 SSKF 的 预测 方程 相同 ,除了 求 和 的 
上 限 外 ,与 FDKF 的 第 二 种 形式 也 相同 。 第 13 章 将 总 结 这 些 Sigma 点 卡尔 曼 滤 
波 器 ,并 且 给 出 过 程 框 图 。 表 11 -1 概括 了 GHKF 过 程 。 
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表 11 -1 多 维 高 斯 - 厄 米 特 卡 尔 曼 滤波 器 过 程 


滤波 器 初始 化 | 初始 化 和 xz A Py 


X9*na4 z  — * 5D, ar J ku 0,1 765,39. =j 
步骤 (2) 状态 矢量 预测 
步骤 (3 ) 观测 值 预测 

is = Sani 下 K, (zh E Zun-_i ) 


卡尔 曼 滤 波 器 
4 
步骤 (4) 更 新 
Fae = Paol - Kl - PET Q 


存储 结果 [iiu FL PT, 存储 到 跟踪 文件 中 


11.4 高 维 /高 阶 多 项 式 的 稀 朴 网 格 逼 近 


如 上 所 述 ,高 斯 - 厄 米 特 滤波 器 所 需 网 格 数目 为 
N, = mn (11 -95) 


式 中 :m OS A a a BYR TE SAS PG; n, 为 状态 矢量 的 维度 。 当 状态 矢 
量 的 维 数 增加 时 ,所 需 Sigma 点 的 数量 呈 指 数 级 增长 ,导致 “ 维 数 灾 难 ”; 当 维 数 
大 于 4 时 ,完全 填充 的 方形 网 格 的 Sigma 点 数目 变 得 非常 大 ,此 时 很 少 使 用 
CHKF 滤 波 器 。 本 章 前 面 几 节 , 已 经 给 出 二 阶 (m 2 3) JUOK SERT 85 E 3": 4 
Sigma 点 ,对 于 这 种 情况 ,减少 点 的 数目 的 唯一 方法 是 用 UKF 代替 GHKF ,不 过 这 
样 会 损失 一 些 精 度 。 

在 式 (11 -30) 前 述 内 容 可 知 , 厄 米 特 插值 方法 可 以 推广 至 高 阶 。 当 然 ,高 
阶 插值 需要 更 多 的 Sigma 点 ,使 维 数 灾难 的 情况 更 为 严重 。 由 于 高 阶 厄 米 特 插 
值 需要 求解 与 厄 米 特 多 项 式 同 阶 的 多 项 式 方程 ,通常 采用 数值 方法 来 生成 特征 
值 ,得 权 值 和 Sigma 点 ,用 无 理 数 替 代 式 (11 -48) 中 的 有 理 数 。 文 献 [7] 给 出 了 
这 些 无 理 数 的 列表 。 然 而 , 当 使 用 数据 列表 时 必须 注意 , 权 值 是 为 非 规范 化 的 厄 
米 特 多 项 式 而 生成 的 。 









^ 3nx-1 ; 
Xnjn-l = u; fuu) 


Print = Xa cU usa) - 45-1] 





x Uf Qus) 一 nl T +Q 






xd = PNP + BD,,,_ r? J = 0,.1,--. 3 = 












A 3n,-1 ~ ; 
Znin-l = Lites wh Qd si) 








Prin = Lio laa) - Zin! Jiha) = Zus-i] +R 


Py. Ya Urdu) — X454] ° [hdi = Zaa I" 





K, 2 F2 (Piai ) - 
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多 维 正 交 积 分 的 最 新 进展 - Smolyak” 的 工作 ,使 得 高 阶 高 斯 - 厄 米 特 积 分 更 
易于 实现 。Smolyak 提出 一 种 多 维 正 交 积 分 的 数值 方法 ,以 牺牲 准确 性 为 代价 , 减 
少 了 网 格 点 的 数量 。 完 整 的 减少 网 格 点 的 数学 方法 介绍 可 参考 文献 [9 - 13 ] 。 
上 面 介绍 的 厄 米 特 Sigma 点 的 生成 方法 符合 Smolyak 的 方法 ,这 里 Sigma 点 根据 
它们 与 原点 的 距离 ,按照 层次 进行 分 组 ,在 式 (11 - 81) 一 般 维度 情况 下 可 以 看 
到 。 对 稀 玻 网 格 数值 积分 ,必须 取 整 个 网 格 的 一 个 子 集 ,并 重新 归 一 化 缩减 的 权 
值 集合 。 可 惜 , 这 种 稀疏 网 格 积分 方法 不 能 减少 厄 米 特 二 阶 多 项 式 所 需要 的 
Sigma 点 的 数量 。 

例如 , 仅 需 要 两 个 最 低级 的 网 格 点 ,r” =[0] e R",j=0 Mr” =[1] e 
R",j =1,…,n,。 初 始 权 值 为 wo = (2/3)“ 和 w= (2/3) (1/6),j=1,…,n,o 
每 个 权 值 必须 除 以 所 有 的 权重 的 总 和 (2/3)” C[4 +n,]/6) ,实现 归 一 化 。 归 
一 化 后 的 权 值 为 wo 74/ (4 * n, ) fll v; 2 1/(4 * n,) jj 213,7, n,o 这 样 稀 朴 高 斯 - 
厄 米 特 滤 波 器 与 式 (9 -24) \ 式 (9 -25) fj UKF 在 形式 上 相同 ,但 是 有 


"m = [ro j=0,1,-+-,2n, (11 - 96) 


4/(4 +n) j =0 
» =| (11 -97) 


—w, j = 1,+,2n, 


4 


以 类 似 的 方式 ,可 以 从 式 (11 -81) 中 得 尽 可 能 多 的 Sigma 网 格 点 的 子 集 ,并 
且 在 将 权 值 归 一 化 后 ,生成 另 一 种 稀 朴 网 格 GHKF 滤波 器 。 该 方法 使 得 稀 朴 网 
格 CHKF 适用 于 高 维 情况 。 此 外 ,对 于 高 度 非 线性 情况 ,可 使 用 高 阶 厄 米 特 多 
项 式 来 得 高 阶 稀疏 网 格 GHKF 滤波 器 ,以 合理 的 计算 成 本 提供 更 高 的 精度 。Jia 
等 “首次 将 这 一 方法 应 用 到 航天 器 姿态 估计 。 在 他 们 的 论文 中 给 出 了 不 同 稀 
LSE GHKF 滤波 器 所 需 点 的 数目 的 列表 ,其 中 厄 米 特 多 项 式 达 到 7 阶 ,状态 矢量 
达到 6 维 。 


11.5 GHKF 在 DIFAR 船舶 跟踪 的 应 用 案例 分 析 


对 于 DIFAR 跟踪 问题 ,动态 方程 是 线性 的 ,处 理 这 一 问题 用 到 的 GHKF 过 程 
与 表 9 -2 所 列 完全 相同 。 对 于 权 值 和 Sigma 点 ,| (wj,c”) ,=0,1,…,n,| 的 值 由 
式 (11 -77) ~ 式 (11 -79) 给 出 ,这 里 取 e? - 3r? 。 在 这 个 GHKF 滤波 器 中 , 采 
用 了 与 EKF 相同 的 初始 化 以 及 动态 噪声 值 9( 分 别 在 7.4.3 节 和 7.4.4 节 中 )。 
比较 100 次 蒙特 卡 洛 输出 轨迹 ,结果 表明 ,在 不 同 信 噪 比 条 件 下 ,CHKF 滤波 器 
的 输出 结果 与 EKF 的 输出 几乎 没有 区 别 ,甚至 连 GHKF 发 散 航 迹 的 数目 与 EKF 
蒙特 卡 洛 跟踪 的 数目 也 相同 ,所 以 这 里 不 再 给 出 GHKF 的 跟踪 结果 比较 图 。 
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第 12 章 
蒙特 卡 洛 卡尔 曼 滤 波 器 


Sigma 点 卡尔 曼 滤波 器 首先 对 状态 矢量 进行 仿 射 变换 ,然后 将 非 线 性 函数 
展开 为 多 项 式 ,将 高 斯 加 权 积 分 简化 为 矩 方程 组 ,通过 求解 方程 组 ,得 到 一 组 权 
值 和 Sigma 点 。 虽 然 Sigma 点 方法 在 一 些 文献 中 被 用 来 通 近 密度 函数 ,但 在 本 
书 的 推导 中 并 没 用 其 进行 高 斯 密度 的 通 近 。 

这 里 介绍 一 种 通过 一 组 蒙特 卡 罗 采 样 允 近 高 斯 密度 的 方法 。 回 顾 
式 (5 -51) ,得 


š fJ f( Ne;0,Ddc (12 -1) 


假设 由 N (ce;0,7) 生 成 一 组 蒙特 卡 洛斯 采样 1c”,j =1,…,N,|。 将 和 N(c;0,7) 表 
示 为 密度 函数 的 离散 允 近 , 即 


Nes0,D = LY ale - e") (12 -2) 


式 中 :6(c - 67 ) 为 c” 处 的 单位 脉冲 质量 函数 。 
将 式 (12 -2) 代 入 状态 预测 方程 (5 -51) ,得 
tn = x > [fco)8(e - c? Jde = LY f(e®) — (12-3) 
当时 N,— oo , Hi fl FE25] (803 AY B r PLAT AE E (BS CARL) TEE 
数 1/N, 是 每 个 样本 的 重要 性 权 值 ,而 不 是 概率 。 用 归 一 化 直方 图 表示 指定 区 


域 ,概率 由 该 区 域 样本 归 一 化 密度 得 。 
利用 式 (5 -53) , 式 (12 - 3) SEX 


- MM ^N LyjG. tet $ Danat”) (12-4) 


使 用 -un-! 的 标准 定义 ,可 有 
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d iai = -— * Boge” (12 -5) 
定义 w A 1/N,, Vj 将 式 (12 -4) 重 写 为 


Kasa = Yx, FP) (12 -6) 


其 状态 预测 方程 与 所 有 Sigma 点 卡尔 曼 滤波 器 具有 相同 形式 。 
其 余 的 蒙特 卡 洛 卡 尔 曼 滤波 器 (MCKF ) 预测 方程 由 Sigma 点 预测 方程 给 
出 , 即 


Pin- = Bw FP int) Mai] I (12 -7) 
[FG 1) -%,),-,]'+@ (12 -8) 

N, 
Zins = 2, whois) (12 -9) 
Pa = Ya (hit. ~ San] (12 -10) 
[h xia) ~ $4,451 +R (12 - 11) 

N, 

Pis > wDfarzusa) -Zu.-1] : (12 - 12) 
[hx ) T EL (12 -13) 

在 后 三 个 预测 方程 中 ,x 小 1 可 写 为 
We zXa.i I. ue (12 -14) 


XB ic" 为 由 NN(c;0,7) 抽 取 的 一 组 新 的 蒙特 卡 洛 样本 。 所 有 这 些 预 测 方程 实 
际 上 都 是 非 线 性 函数 的 样本 均值 和 协 方差 。 

注意 ,MCKF 中 从 不 重复 使 用 蒙特 卡 洛 样本 。 本 书 第 五 部 分 将 给 出 一 些 非 
高 斯 密度 估计 方法 ,反复 重用 蒙特 卡 洛 样本 ,只 是 在 每 次 迭代 中 更 新 它们 的 权 
重 。 这 就 是 术语 粒子 滤波 器 的 来 历 ,每 个 蒙特 卡 洛 样本 都 被 当 作 是 一 个 粒子 ,在 
ERP ERE s 


12.1 蒙特 卡 洛 卡 尔 曼 滤波 器 


表 12 -1 概括 了 MCKF 过 程 。 
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#12-1 蒙特 卡 洛 卡尔 曼 滤 波 器 过 程 ( 译 者 注 : 原 文 此 处 有 误 ) 


0=< o <1 
步骤 (1) 滤波 器 初始 化 
初始 化 加 A Py 


w = 1/N, j= 1,--,N, 












c? ~ N(ce;0,I) j= 1... N 








Wm 7 S nai * Dac j= 1... Ñ 
" N, 
Xun-l = X Sar) 


Pisi T Lom (Fx Punt) - $444] E (Svs) Te 


Sas y + Q 





5 


生成 状态 矢量 







步骤 (2) 













^ 
S 
t 


N(ci0,1) j=1,N, 


c? j 2]1,-,N 


5 





xd. = Knit +D 


nln-1 
Rs = Y w OD) 
nin-l 7 Lomi Lh ) — aa IL TOT P es "FR 


样本 预测 
Pis = Y wi Uus) 724541 > 


生成 观测 值 样本 
3 
步骤 (3) 预测 
[hc ) = £... T 


K, E FLAGS p 
步骤 (4) 卡尔 曼 滤 波 器 更 新 | ,= Enni + Ky (zn 一 zi) 
Pas = Pii - K, Pš ,_, KI 
存储 结果 
2 5 X Pr 
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第 13 章 
高 斯 卡尔 曼 滤 波 器 总 结 


本 章 将 总 结 第 6 ~ 第 12 章 中 提出 的 高 斯 分 布 卡尔 曼 滤 波 器 的 重要 特性 。 
首先 ,分 析 LKF 和 EKF ,给 出 每 种 算法 的 过 程 流 图 ;然后 ,分 析 Sigma 点 类 卡尔 
曼 滤 波 器 ,给 出 通用 的 过 程 框图 ,并 通过 两 个 列表 ,定义 每 种 滤波 器 对 应 的 Sig- 
ma 点 和 权 值 ;最 后 ,从 处 理 效率 的 角度 ,对 每 种 Sigma 点 卡尔 曼 滤 波 器 进行 性 能 
特征 的 评价 。 


13.1 解析 卡尔 曼 滤 波 器 


在 许多 动态 系统 中 ,动态 方程 或 者 观测 方程 是 线性 的 。 本 书 中 大 多 数 案例 
也 符合 这 一 情况 。 

在 某 些 情况 下 ,动态 方程 是 线性 的 ,而 观测 方程 是 高 度 非 线性 的 。 这 时 ， 
通常 用 式 (6 -3) 和 式 (6 -6) 线 性 卡尔 曼 滤 波 器 预测 方程 来 代替 其 他 的 预测 
方法 ,进行 状态 预测 和 状态 协 方差 预测 。 对 于 另外 一 些 跟 踪 问 题 , 当 观 测 方程 
为 线性 时 ,动态 方程 可 能 为 非 线 性 。 对 于 这 类 问题 ,处 理 方 法 与 上 面相 反 , 即 
使 用 非 线性 方法 进行 状态 预测 ,而 采用 式 (6 -9) ~ 式 (6 -12) 线 性 卡尔 曼 滤波 
髓 预测 来 进行 观测 值 预测 。 最 后 ,如 果 动 态 方程 和 观测 方程 都 是 线性 的 , 则 使 用 
LKF 预测 方程 。 

图 13 -1 给 出 了 LKF 过 程 框图 。 

当 动 态 方 程 和 /或 观测 方程 为 非 线 性 时 ,必须 使 用 第 7 ~ 第 12 章 中 给 出 的 
非 线 性 方法 。 过 去 50 年 中 ,使 用 最 多 的 方法 是 EKF。 在 第 7 章 中 已 经 说 明 ， 
EKF 为 非 线性 函数 的 一 阶 泰勒 多 项 式 展 开 得 的 卡尔 曼 滤波 器 ,形式 与 LKF 相 
[n] ,但 是 需要 额外 计算 雅 可 比 和 矩阵 ,图 13 -2 给 出 了 此 方法 的 框图 。 第 7 章 还 给 
出 另外 一 种 二 阶 扩展 卡尔 曼 滤波 器 ,但 需要 额外 计算 Hessian 矩阵 ,这 通常 很 难 
实现 。 
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图 13 -2 EKF 过 程 框图 


13.2 Sigma 点 卡尔 曼 滤 波 器 


第 8 章 中 提出 用 中 心 有 限 差分 近似 取代 雅 可 比 矩 阵 和 Hessian 矩阵 的 计算 , 
以 得 不 同形 式 的 FDKF 滤波 器 。 由 式 (8 -41) \ 式 (8 -43 ) 和 式 (8 -47) 给 出 了 
标准 FDKF 的 协 方差 预测 方程 ,该 方程 与 其 他 的 Sigma 点 方法 预测 方程 在 形式 
上 不 同 。 而 改进 后 的 FDKF 滤波 器 的 协 方差 预测 方程 式 (8 -49)\ 式 (8 -50) 和 
式 (8 -51) ,与 UKF 的 预测 方程 是 完全 相同 的 。 因 此 ,在 总 结 Sigma 点 卡尔 曼 滤 
波 器 时 ,没有 将 FDKF 包含 在 内 。 

Sigma 点 卡尔 曼 滤 波 器 UKF.SSKF.GHKF 和 MCKF ,都 具有 相似 的 
结构 ,因此 可 以 用 图 13 -3 中 的 通用 流程 进行 处 理 , 只 是 c” 和 o, 取 值 有 所 
不 同 。 表 13 -1 和 表 13 -2 分 别 列 出 了 每 种 滤波 器 中 cw” 和 o, 的 取 值 。 基 
于 图 13 -3 的 过 程 框图 ,编写 一 个 通用 的 Sigma 点 卡尔 曼 滤 波 器 子 程序 相对 
比较 容易 ,该 程序 可 调用 二 级 子 程序 ,从 表 13 - 1 和 表 13 -2 中 选取 合适 的 
Sigma 点 和 权 值 。 
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图 13 -3 通用 Sigma 点 卡尔 曼 滤波 器 过 程 框图 


表 13 -1 Sigma 点 卡尔 曼 滤 波 器 数据 汇总 :第 1 部 分 
一 用 于 Sigma 点 卡尔 曼 滤 波 器 的 e 的 形式 
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& 13-2 Sigma 点 卡尔 曼 滤波 器 数据 汇总 :第 2 部 分 
一 Sigma 点 权 值 的 形式 
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一 些 Sigma 点 卡尔 曼 滤波 器 具有 维 数 灾难 问题 , 即 当 状态 矢量 的 维 数 增加 
时 ,积分 点 的 数目 呈 指 数 级 增长 。 表 13 - 3 中 列举 了 每 种 滤波 器 所 需 积 分 点 的 
数目 。 如 表 所 列 ,对 于 高 维 的 状态 矢量 ,从 计算 的 角度 最 好 的 过 滤器 是 SSKF ,其 
次 是 UKF。 当 维度 大 于 4 时 ,任何 问题 都 不 会 用 GHKF 来 处 理 。 对 于 高 度 非 线 
性 动态 方程 ,每 个 滤波 器 的 精度 通常 是 由 积分 多 项 式 展 开 的 阶 数 所 决定 。 因 此 ， 
MCKF 的 精度 最 高 ,只 要 随机 样本 数量 足够 高 , 非 线性 函数 中 不 会 用 到 任何 近似 ; 
其 次 是 GHKF ,其 使 用 最 高 的 多 项 式 阶 数 ;第 三 是 UKF ,其 使 用 三 阶 多 项 式 ; 最 后 ， 
最 不 精确 的 是 SSKF。 因 此 ,对 于 给 定 的 估计 问题 ,最 好 对 所 有 滤波 器 进行 试验 , 然 
后 在 计算 量 约束 内 ,选择 均 方 根 误差 最 小 的 滤波 器 。Wu 等 "的 论文 中 可 以 找到 
关于 上 述 滤波 器 以 及 相关 其 他 滤波 器 在 精度 ,效率 和 稳定 性 方面 的 讨论 。 

表 13 -3 Sigma 点 卡尔 曼 滤 波 器 所 需 积 分 点 数目 的 比较 
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列表 13 - 1,3€ 13 -2 给 出 任意 状态 矢量 维度 下 ,为 所 有 的 Sigma 点 卡尔 曼 
滤波 器 生成 集合 ji,c” 必 …1,……,NV,| 的 MATLAB 子 程序 。 第 一 个 子 程序 分 别 
根据 表 13 -1 和 表 13 -2 中 的 公式 ,计算 权 值 和 Sigma 点 。 该 程序 有 4 个 输入 ， 
分 别 为 :标志 位 (t_flag) 指 明 是 针对 哪 种 Sigma 点 卡尔 曼 滤 波 器 计算 Sigma 点 和 
权 值 ;状态 矢量 维 数 (L=n, ) 为 Sigma 点 (或 采样 点 ) ) 的 个 数 ;LL, 由 表 13 -3 计 
算得 ;最 后 是 数值 流 ,为 随机 数值 发 生 器 的 流 。 第 二 个 子 程序 实现 了 高 斯 - 厄 米 
特 矢 量 点 的 生成 ,由 美国 约翰 ' 霍 普 金 斯 大 学 应 用 物理 实验 室 Neill Bryant Bel- 
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tran 5j 5j , ARGA HAR RE u HEROJ n 的 Sigma 点 。 为 实现 这 一 功能 ,用 到 了 二 
进 制 数 及 其 组 合 。 首 先 , 生 成 所 有 可 能 二 进 制 数 ,其 位 数 与 维度 相同 ;其 次 ,对 这 
些 二 进 制 数字 进行 筛选 , 仅 保 留 有 z， 个 "17 的 二 进 制 数 ;再 次 ,生成 一 个 负 值 的 
掩 码 , 其 中 包括 1 和 -1 所 有 可 能 组 合 ;最 后 ,结合 “形状 掩 码 ” 和”* 负 值 掩 码 ”， 
找 出 高 斯 - 厄 米 特 矢量 点 的 所 有 可 能 组 合 。 
列表 13 -1 生成 权 值 和 Sigma 点 
function [w,c] =WeightsSigmaPoints(t_flag,L,LL,Stream) 
w =zeros(1,LL); 
switch tflag 
case}|1} % UKF 
w(1) =0.25; 
w(2:LL) =(1 - w(1))A2*L); 
r =zeros(2 *L+1,L); 
r(2;L+1,;) =eye(L); 
r(L+2:2 *L+1,;) = -eye(L); 
c=sqrt(LA1-w(1)))*r; 
case}2} % SSKF 
w(1) 20.25; 


w(2:LL) =(1 - w(1))AL+1); 
c =zeros(LL,L); 
c(2,1) -1/sqrt(2*w(2)); 
c(3,1) = -1/sart(2*w(2)); 
for n =2:L 
for q=1:n 
e(q+1,1:n) =[c(q+1,1:n-1)1/+ 
sqrt(n*(n«1)*w(2))]; 
end 
c(n«2,1;n) -[zeros(1,n-1) -nA 
sqrt(n*(n+1)*w(2))]; 
end 
case}{3} % GHKF 
facti =2/3; 
fact2 =1/6; 
r-zeros(LL,L); 
w(1) = fact1 “L; 
q=1; 
for u=1:L 


rr =vectorPointGenerator(u,L); 
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k =size(rr,1); 
r(q+1:q+k,:) =rr; 
w(q+1:q+k) -(factl^(L-u))*(fact2^u); 
q=q+k; 
end 
c =sqrt(3) * r; 
casel4]| $ MCKF 
w -ones(1,LL) /LL; 
c -randn(Stream,LL,L); 
end 
列表 13-2. 矢量 点 发 生 器 
function out =vectorPointGenerator(u,n) 
% Written by 
% Neill Bryant Beltran 
% 8 November 2010 
i Ë ú = = 0 
out =zeros(1,n); 
return 
end 
% Find a,, binary numbers with same number 
% of digits as there are dimensions,n 
shape mask =dec2bin(2*n-1:-1:;1) == '1'; 
shape size -size(shape mask); 
$ Remove a, vectors that do not have u '1's 
for ii =shape_size(1): -1:1 
i f nnz(shape mask(ii,:)) =u 
shape_mask(ii,:)=[]; 
end 
end 
shape_size =size(shape_mask); 
% Create the negative mask 
neg mask =(dec2bin(0:2*u-1) = = '1') *1.0; 
neg mask(neg mask = = 0) = -1; 
neg mask = -1*neg mask; 
neg size -size(neg mask); 
out = zeros( shape. size(1) * neg size(1),n); 
% Combine the shape mask and negative mask 
for ii »1;neg size(1) 
for jj =1:shape_size(1) 
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kk =1; 
for 11 =1 :了 
if shape mask(jj,11) 
continue 
end 
out((ii-1) * shape_size(1) «jj,11) =… 
shape_mask(jj,11) *neg mask(ii,kk); 
kk =kk +1; 
end 
end 
end 
在 文献 [2 -7] 中 可 以 找到 计算 高 斯 加 权 积 分 的 其 他 算法 。 文 献 中 讨论 的 
许多 算法 的 实际 应 用 类 似 于 前 面 提出 的 Sigma 点 卡尔 曼 滤波 器 。 与 第 8 ~ 第 12 
章 给 出 的 方法 相 比 ,它们 大 部 分 阶 数 比 较 高 ,因此 与 UKF 相 比 需要 更 多 积分 点 ， 
所 需 点 的 数目 介 于 UKF 和 GHKF 之 间 。 在 Candy[ 8] 的 书 中 ,给 出 了 更 为 经 典 
的 Sigma 点 卡尔 曼 滤波 器 的 推导 ,采用 回归 方法 ,类 似 于 卡尔 曼 滤波 器 的 原始 
推导 。 


13.3 一 种 更 实用 的 运用 卡尔 曼 滤 波 器 族 的 方法 


通过 各 种 案例 分 析 , 对 式 (3 -17) 和 式 (3 -18) 给 出 的 动态 模型 和 观测 模型 
进行 仔细 检查 ,发 现存 在 一 个 层次 模型 。 表 13 -4 给 出 了 这 个 层次 模型 。 
表 13 -4 动态 模型 和 观测 模型 层次 


基于 此 模型 的 层次 结构 ,可 以 认为 动态 预测 过 程 是 完全 独立 于 观测 预测 过 
程 的 。 

这 使 人 们 能 够 选用 每 种 模型 最 适合 的 卡尔 曼 滤 波 方法 。 例 如 ,对 于 线性 / 非 
线性 情况 ,可 以 使 用 线性 卡尔 曼 滤波 器 方程 进行 动态 预测 ,而 采用 非 线性 卡尔 曼 
滤波 器 方法 进行 观测 预测 。 同 样 , 对 于 非 线性 / 非 线性 情况 , 当 状 态 矢量 x, 为 高 
维 而 观测 矢量 z, 是 低 维 时 ,可 以 使 用 UKF 进行 动态 预测 ( 因为 GHKF 不 实用 ) , 
而 使 用 GHKF 进行 观测 预测 ( 因为 它 通常 是 最 准确 的 ) 。 上 述 讨 论 表明 了 卡尔 
最 滤波 器 架构 的 灵活 性 ,在 解决 具体 问题 时 ,可 以 建立 最 高 效 的 估计 过 程 ,进行 
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实际 应 用 。 当 然 ,从 软件 实现 的 角度 来 看 ,这 也 许 并 不 是 最 高 效 的 方法 。 
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第 14 = 
ERS} 波 器 族 的 性 能 度量 


本 章 将 讨论 一 些 通用 的 方法 来 度量 第 6 ~ 第 12 章 提 出 的 卡尔 曼 滤波 器 的 
性 能 。 性 能 度量 方法 分 为 两 类 ,第 一 类 适用 于 无 法 得 到 实际 轨迹 数据 或 者 针对 
特殊 场景 ,例如 在 对 少量 的 现场 试验 的 分 析 中 ,可 得 到 一 些 实际 轨迹 数据 ,但 统 
计 意 义 并 不 明显 。 对 于 这 种 情况 ,只 能 将 滤波 器 估计 轨迹 协 方差 矩阵 作为 输入 ， 
生成 连续 的 跟踪 误差 椭圆 ,然后 进行 性 能 度量 。 第 二 类 适用 于 用 仿真 来 生成 一 
个 完全 已 知 的 目标 轨迹 ,这 样 就 可 以 进行 较 多 的 性 能 度量 。 这 是 因为 可 以 生成 
大 量 的 蒙特 卡 洛 观 测 数据 集 , 这 样 就 能 实施 各 种 跟踪 性 能 度量 方法 ,如 均 方 根 
(RMS) 误差 .Cramer - Rao 下 界 (CRLB ) 和 发 散 轨 迹 比率 等 。 本 章 将 对 所 有 的 性 
能 度量 方法 以 及 每 种 方法 相关 的 具体 实施 问题 进行 讨论 。 


14.1 误差 椭圆 


跟踪 估计 滤波 器 的 应 际 应 用 中 ,当真 实 状 态 为 未 知 ,比如 在 一 个 现场 系统 中 
跟踪 一 般 真 正 的 船舶 ,或 者 当 只 进行 了 少量 的 现场 试验 、 仅 知道 大 约 的 真实 状态 
时 ,最 好 的 衡量 跟踪 滤波 器 性 能 的 方式 是 利用 滤波 器 的 估计 协 方差 矩阵 输出 来 
生成 并 绘制 滤波 跟踪 状态 的 误差 椭圆 。 由 于 屏幕 显示 所 固有 的 二 维特 性 ,为 清 
楚 描述 ,这 里 仅 采 用 二 维 笛 卡 儿 坐 标 系统 下 的 位 置 。 

在 任意 离散 时 刻 t, ,跟踪 状态 x, 的 概率 密度 函数 为 高 斯 分 布 , 即 

X, = N (z, uL PT.) (14-1) 

RP: (x Prin) APR F ILA ERR PASE AR BEE EE ME, AR IER 

间 下 标 , 只 检查 由 x 2 [n.n ]^ 指定 的 二 维 位 置 分 量 。 如 图 2 -9 所 示 ,常数 概率 

( 仿 射 变换 之 前 ) 的 椭圆 轮廓 一 般 沿 主轴 方向 拉 长 ,也 可 能 是 倾斜 的 ,这 样 椭圆 

的 轴 与 状态 位 置 矢 量 的 轴 不 平行 。 下 面 的 步骤 中 将 回答 问题 , 即 给 定 一 个 包含 
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概率 ,如 何在 与 状态 矢量 相同 的 坐标 系 中 绘制 误差 椭圆 。 这 里 包含 概率 相当 于 
常数 概率 的 轮廓 。 


14.1.1 正则 椭圆 
位 置 分 量 概率 分 布 的 指数 可 以 表示 为 
s = (x -£)'P'!(x — £) (14 —2) 
式 中 :P 为 位 置 分 量 的 误差 协 方差 和 矩阵。 误差 椭圆 定义 为 
ix = £)' P(e = 2) = C (14 -3) 


Xr iC! 为 包含 区 域 ,取决 于 要 得 到 的 包含 概率 P. ,后面 将 给 出 P. 的 定义 。 
首先 ,将 式 (14 -3) 改 写 为 椭圆 方程 的 标准 形式 。 对 P 进行 特征 值 分 解 , 得 


P = VDV' (14 -4) 
式 中 :D 为 一 个 包含 P 的 特征 值 的 2 x2 对 角 和 矩阵 B] 
A, 0 
p | | (14 -5) 
0 A, 
而 V 是 一 个 由 P 的 两 个 特征 矢量 构成 的 2 x 2 ERE, BD 
V A [v,,v,] (14-6) 
式 中 :mw fll v, 为 2 x1 列 矢量 。 很 容易 证 明 , 对 于 一 个 对 称 、 非 奇异 矩阵 了 ,有 
P'asyp*y' 
式 (14 -3) 可 重 写 为 
(x - *£)'VD V(x - £) < C (14 -7) 
^ 
y À V'(x - X) (14 -8) 
将 式 (14 -7) 重 写 为 
y!D`'y = a + zi (14 -9) 
选取 A, 作为 最 大 的 特征 值 ,并 做 如 下 定义 , 即 
a & C Ai (14 -10) 
bà C SA, (14-11) 
选取 式 (14 -7) 中 的 等 式 ,得 正则 椭圆 方程 为 
+ + 站 = 1 (14 -12) 
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14.1.2 确定 书 的 特征 值 
为 确定 对 称 和 矩阵 P 的 特征 值 与 特征 矢量 ,检验 特征 值 方程 
Py = Av (14-13) 
[P-Al]v = 0 (14 - 14) 
多 项 式 f( 和 A) = |P - AT1 被 称 为 P 的 特征 多 项 式 ,f(A) = 0 的 根 即 为 特征 值 
"WE 


= 0 (14 - 15) 








得 如 下 解 





I r 
P,, +P 
à, = - JP, = Pn)? + APA, (14 -17) 


14.1.3 确定 误差 椭圆 旋转 角度 
对 于 对 应 特征 值 A, 的 特征 矢量 v , 式 (14 -14) 变 为 





Ë "^o TR I" - 0 (14 - 18) 
Py Py - A, 
4 
vy À [^] (14 - 19) 
12 
», À [^] (14 -20) 


IP: (é £12) RI C£, »En ) 分 别 为 Vi 和 v; AY Fi EL A b Eo 已 知 £u ,通过 
式 (14 -18)5R £,,18 


Py 
E12 =- po A (14 -21 ) 


图 14 -1 给 出 了 椭圆 相对 于 笛 卡 儿 轴 的 旋转 角 。 由 图 可 知 ,误差 椭圆 旋转 角 
度 为 


@ = tan (£2) = tan Í = it) (14 -22) 
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图 14 -1 误差 椭圆 旋转 角度 


14.1.4 确定 包含 区 域 


由 于 x 的 分 量 是 联合 高 斯 分 布 的 ,如 式 (14 -1) 可 知 ,y 的 分 量 同样 如 此 , 因 


此 有 
y ~ N(y;0,D) 
y 的 概率 密度 函数 可 写 为 
py) «2-1 
2m Aida 


因此 ,包含 概率 可 以 表示 为 


2 2 
P. Pp? Y2 < cel (y)d 
C A, * Ña Í p \y)ay 


4,2 <C 
Ai A2 
令 
2 2 
rà Hoa «C 
À, 2 
得 
y, = WAircosp 


y; = WAzrsinp 


dy,dy, = ./A,A,rdrdg 
包含 概率 式 (14 - 25) ABH 
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(14 -23 ) 


(14 -24) 


(14 -25) 


(14 -26) 


(14 -27) 
(14 -28) 
(14 -29) 


PC = +f def re Yar " fre? ar aj=e7* — (14-320) 
对 C 进行 求解 ,得 
C = /-2In(1 - PC) (14 -31) 
因此 , 选 定 一 个 包含 概率 就 完全 决定 了 C 的 取 值 。 
14.1.5 误差 椭圆 参数 化 绘图 
一 个 正则 椭圆 (形式 为 式 (14 - 12)) 可 以 表示 为 点 (x.(08),y.(0)) 的 轨 
迹 , 即 
x,(0) = x, + acos0 cosp — b sin @sing (14 -32) 
y.(0) = y, +acosg sing + b sin gcosop (14 -33) 


X Lx, y, ) 为 椭圆 中 心 ,g 为 笛 卡 儿 坐 标 系 x 轴 与 椭圆 主轴 的 夹 角 ,参数 矢量 0 
定义 为 行 撩 量 8 人 [0,0.01,…,2T]。 


定义 参数 矩阵 
Q(0) = bn] = [° 9 (14 -34) 
q,(0) sin 0 
由 式 (14 -10) 和 式 (14 — 11) ,可 以 证 明 
A, O0 
| "J. [®™ |= co" (14 -35) 
0 b 0 Jis 
: [^ is _ E "i (14 -36) 
sing coso En &x 


可 以 将 椭圆 轨迹 的 参数 方程 写成 矢量 - 矩阵 形式 


x,(0)) pê] [£nC Arg, (0) +£,C VA2q,(0) 

«o = [= DI: | 

y(0) ^Y LEC /A,q,(0) + £C Arg, (8) 
(14 -37) 

这 里 将 椭圆 中 心 设 为 (4,7) 。 或 者 ,可 以 将 参数 方程 表示 为 
x,(0) = £ + VCD'2Q(0) (14 -38) 
£A[x,*] 

附注 14.1 可 调用 MATLAB 函数 [V,D] =eig( 已 ) 分 别 得 特征 值 Y 和 特征 


矢量 矩阵 D. 
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在 第 卡 儿 跟踪 估计 图 基础 上 绘制 生成 误差 椭圆 数据 的 步骤 如 下 : 

(1) 选取 包含 概率 Pc 的 值 ,通常 为 0.9, 用 式 (14 -31) 计 算 C 值 ; 

(2) 生成 参数 矢量 05 [0,0.01,--,2«], BR 0.01 之 外 ,间隔 也 可 以 取 其 他 
任意 值 ; 

(3) 由 式 (14 -34) 生 成 Q( 0) ; 

(4) 生成 协 方差 矩阵 2, 的 特征 值 Y 和 特征 矢量 矩阵 也 ; 

(5) 生成 x.(0) =£,, + VCD'"Q(0). 


14.1.6 误差 椭圆 举例 


图 14 -2 绘制 了 DIFAR 实例 中 UKF 跟踪 输出 中 每 100 个 数据 点 对 应 的 误 
差 椭圆 。 当 真实 航 迹 未 知 时 ,通过 这 种 标 绘 方法 ,可 以 沿 轨迹 上 各 点 进行 跟踪 估 
计 , 并 评价 估计 的 不 确定 性 。 


4 
3 
2 
l 
0 


y- 位 置 (n mile) 


-3 

-4 

-2 -15 -1 -05 0 0.5 1 1.5 
xX- 位 置 (n mile) 


图 14-2 在 DIFAR 实例 中 UKF 跟踪 输出 上 和 春 加 绘制 的 误差 椭圆 示例 


14.2 1597; fRiX Z5 


利用 式 (3 - 16) ,在 已 时 刻 , 可 以 将 状态 协 方差 矩阵 定义 为 
PS = el (x, ~%,,)(%, = 部 ) | (14 -39) 
假设 在 仿真 当中 ,x, 已 知 ,而 x,, 是 无 偏 的 ,对 角 元 素 P5, 的 平方 根 可 以 由 条 件 
均 方 根 误差 (或 者 误差 平方 均值 的 平方 根 ) 来 近似 。 
在 第 4 章 中 曾 强 调 , 仿 真 的 首要 任务 是 对 所 有 关注 的 时 刻 |,,n =0,1,…， 
N} ,生成 一 个 真实 的 状态 轨迹 ix,,n =0,1,…,N|。 由 真实 状态 轨迹 可 生成 真 
实 观测 轨迹 ,在 每 个 观测 值 中 加 入 独立 噪声 ,以 得 一 组 仿真 观测 值 |z, n =0,1,…， 
N| 。 为 计算 均 方 根 误差 ,用 蒙特 卡 洛 方式 生成 多 组 观测 值 , 这 样 就 有 了 M 组 独 
立 的 观测 值 |z, , ;n=0,1,…,N;m =1,2,…,M|。 注 意 这 里 的 观测 值 品 声 必须 
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独立 于 时 间 采 样 点 (zi ,必须 独立 于 zw, V i)) 和 蒙特 卡 洛 运行 (z, ,必须 独立 于 
nk? V lk) o 

用 给 定 的 跟踪 滤波 器 处 理 各 组 蒙特 卡 洛 观测 值 ,以 产生 一 组 跟踪 文件 输出 
X, Cm) 和 在 nn 处 的 均 方 误差 矩阵 ,得 


M 


S(&,.) à p Y [x, -£, (m)] [x, c£, (m)]' (14-40) 
m=1 
均 方 根 误差 可 写 为 
. É : ñ 412 
#as(z,) = diag{ [45 Y, (x, - £001 [zx - 4,001]. ] 


(14 -41) 


这 里 diag} - i spei peg i gg P Cc dac E 
一 个 4 维 状态 矢量 x, = [xn -—9— uu 


Crs (Xin) 


erms ( X2 ) 


€xus(X,) = (14 -42) 


erms ( X , ) 


erus ( Xan ) 


1 M n" 1⁄2 
erms( Xin) A [>> ETA = Sinin (m) ]° | (14 -43) 


14.3 发 散 轨 迹 


在 计算 均 方 根 误差 性 能 时 必须 注意 ,因为 即使 是 100 次 蒙特 卡 洛 运行 中 的 
1 个 发 散 轨迹 ,也 会 造成 均 方 根 误差 统计 的 偶 差 。 可 以 将 发 散 轨迹 看 作 蜡 常 值 ， 
它 不 符合 高 斯 假设 , 即 均 方 根 误差 是 标准 偏差 的 无 偏 采样 数据 估计 。 不 过 ,发 散 
轨迹 的 判断 是 一 个 主观 的 决定 ,通常 与 实际 场景 相关 。 

对 于 DIFAR 案例 性 能 分 析 , 如 果 % 轴 或 y 轴 的 估计 值 在 最 后 时 刻 偏离 真实 
值 大 于 2nmi, 就 判定 估计 轨迹 为 发 散 轨迹 。 由 第 7 ~ 第 12 章 内 容 可 知 ,在 DI- 
FAR 实例 中 , 当 信 噪 比 低 于 SdB 时 , 随 着 信 噪 比 的 降低 ,发散 轨迹 的 数目 增加 。 


14.4 Cramer -Rao FA 


有 一 个 问题 ,给 定 场景 (一 组 动态 模型 和 观测 模型 以 及 状态 轨迹 ) 下 ,可 得 
到 的 最 好 的 (最 小 的 ) 均 方 根 误差 是 多 少 ? 因为 总 是 存在 观测 噪声 , 当 使 用 的 动 
态 模型 中 没有 任何 动态 噪声 时 ,就 可 以 得 到 最 好 的 均 方 根 误差 。 
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那么 下 界 有 什么 用 呢 ? 评估 可 行 的 估计 算法 (跟踪 器 ) 的 性 能 可 能 很 困难 ， 
而 下 界 会 给 出 估计 性 能 指标 。 在 许多 情况 下 ,需要 用 性 能 下 界 来 确定 系统 性 能 
需求 是 不 是 能 够 实现 。 

本 节 大 部 分 的 内 容 是 基于 文献 [1 -3] 及 其 所 引用 的 参考 文献 。 本 节 主 要 
关注 Cramer - Rao 下 界 ,但 文献 [2] 超 出 这 一 界限 ,研究 用 各 种 不 同 的 边界 来 解 
决 问题 ,而 不 局 限于 本 书 的 贝 叶 斯 估计 方法 。 


14. 4.1 一 维 情况 


考虑 一 维 情况 ,如 果 过 程 (动态 ) 品 声 为 零 , 则 x 是 确定 的 。 这 样 ,唯一 会 导 
Be x 的 误差 的 是 观测 噪声 ,可 以 用 似 然 函 数 来 表示 。 对 于 规范 化 的 似 然 函数 ,有 


fp! x)dz = 1 (14 -44) 
x(z) 的 期 望 值 由 下 式 给 出 , 即 
fip z)dz = x (14 -45) 
以 此 为 基础 ,由 于 x 值 确定 , 式 (14 -45) 可 以 写成 
Jc - x)p(z| x)dz = O (14 - 46) 
关于 x 求 导 , 可 推导 出 
=ë - x)p(z1 x)dz = O (14 -47) 
R üp(zl x) 
- [p(z1 x)dz + [(% - x) =e = 0 (14 -48) 
Ox 
J - x) prr aia [pc x) dz (14 -49) 
Ox 
Ja 2) PE De = 1 (14 -50) 
Ox 
注意 
a _ 1 Əp(zl x) 
ga nP (zl x) = sisl a] 7 (14-51) 
st (14 -50) 变 为 
[G x)p(z1 x) 2-Inp(z | x)dz 21 (14 -52) 


l = Jf - > vp(z| x) | [npt x) vp(z| x)|dz (14-53) 
Ox 


对 等 式 两 边 求 平方 ,得 
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1 = [ft - o) VPT a7] [ Sint 2) PGTA |dz} (14 -54) 
利用 柯 西 不 等 式 , 得 
I< [c - x)?p(zl x) def [ S np | x)| pz x)dz (14-55) 
TERES, CELER 
JL mpi x)| PGi x)dz 
3k (14 - 56) 的 左边 为 变量 o, 而 右边 的 分 母 可 看 作 是 Fisher 信息 J à 
e([ mci] }, 这 样 得 


JG - x)?p(z x)dz z (14 -56) 


2 l 


l 
dad (14 -57) 
ef [Zn] x) | | 7 
这 里 将 一 维 CRLB 定义 为 
(RU às (14 - 58) 


e{ [Zinc] 2] } 


因为 在 蒙特 卡 洛 仿真 中 ,可 以 用 均 方 根 误差 估计 得 方差 ,这 样 CRLB 的 平方 根 就 
成 为 均 方 根 误差 的 下 界 。 


14.4.2 多 维 情况 
对 于 多 维 情况 ,Fisher 信息 变 为 Fisher 信息 矩阵 J,, ij CRLB EMIS". n 
果 在 状态 动态 模型 中 包含 过 程 噪声 Fisher 信息 矩阵 的 逆 就 是 PLAY F L , 即 
PS. A s|[x, —š,.] [zs - š,.]'] 2S (14 -59) 
将 真实 状态 轨迹 定义 为 ,4a {Xo ,Xi ，,…,x,| ;状态 估计 初始 化 为 xo, ERA E 
何 观 测 值 之 前 ,概率 为 p (xo) ;使 用 观测 值 z A 129,2 nz.) RAS x, 在 时 刻 


t, 的 无 偏 估计 由 滤波 器 输出 确定 xX, ;轨迹 xx,,, 的 CRLB 由 轨迹 信息 和 矩阵 J, BS 
逆 给 出 ,有 


el [zs 一双] [xi 75,5, ]'1 2 Ji, = CRLB 
将 式 (14 -57) 推 广 至 多 维 情况 ,得 儿 ,, 的 第 一 种 形式 为 
Jin & sl[Y, Inp(xi,,z,,)] [Vslnp(zz)] |] (14-61) 
注意 式 (14 -51) 可 写成 多 维 形式 为 


(14 -60) 


traj 
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1 T 
— V 
PO USA ^ 


将 式 (14 -62) 代 入 式 (14 —61) ,得 几 ,, 的 第 二 种 形式 为 
Jin å- &|V, Ve, Inp(x,.,z..) | 
在 这 里 ,必须 注意 : 


V. Inp(x,,, T2 ) 一 


DOG, Z)... ) (14 - 62) 


(14 - 63) 


(1) 如 果 目 标 状态 包含 过 程 噪 声 w REBEL ERK p(x), Zin) 5 
(2) 如 果 目 标 状态 是 确定 性 的 ( 零 过 程 噪 声 ) ,选取 p(z,, 1x,,); 


(3) Va KIR x, TAMARA. 
14.4.3 递归 法 求解 CRLB 


EJ, 7l J,., ,已 知 的 情况 下 ,求解 几 ,非常 有 意义 ,Tichavsky 等 "就 提出 


了 这 样 一 种 方法 。 注 意 ,可 以 将 x, RIY, SH 


Xini 
Xin 一 
X, 


Á aint By niin 
A ee 
Bu Cain 
Aat) & 7 elV, ,Ve lnp(x,,, Ba | 
Bin ES m cj V, Inp(x,,, Z...) | 


Gus à ia FLAA Inp(x,,, Za) | 


利用 式 (2 - 19) 求 得 分 块 矩阵 (14 -67) 的 逆 , 求 得 JL, BD 


gata KÉ ij 
l;a 一 
T, T, 
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(14 -64) 


(14-65) 


(14 -66) 


(14 -67) 


(14 -68) 
(14 -69) 
(14 - 70) 


(14 -71) 


T, & (Ai asa 7 B C.H. y" (14 -72) 


l:n-1 n ^ nl n*^ 1:n-lln 


T, 全 一 (Can = PETE ATN, u ia 


(14 -73) 
T, à - C4 Br us(Aucunsi 7 B,.- u .C3 Bi. na)™ (14-74) 
T, A (Cun - Bint Aign-tiin-tBire-tin) (14 -75) 
矩阵 J PAT F fü ien io J; 79 X, ff Cramer - Rao 下 界 , 即 
Ja! = (Can - B, aAa- Bina) (14 -76) 
因此 有 
J = Cus -Binnen (14 -77) 


注意 , 式 (14 -76) 需 要 对 一 个 大 型 矩阵 4,,。i*- ETRA i RIK 
个 问题 ,本 书 提出 一 种 递归 关系 ,由 几 得 j。-, 而 不 需要 对 大 和 矩阵 求 道 。 考 虑 
式 (14 -61 ) 中 的 联合 密度 ,有 


p(x,,2,,) = p(xX,,2,,X1, 152,4) 
= p(z, | x,,Xiuasz,L Lap l Xini Zaa) 
pU iE, (14 - 78) 
由 于 z, 只 取决 于 x, ,而 x, 只 取决 于 zi, 式 (14 -78) 变 为 
p(xy,,2,,) = p(z, | x,)p(x, | Xni )p(x;,,,-, a (14 - 79) 
若 将 中 ,分解 为 


AR 
Vs Ns = . La E v] 
v, 


v w gy gp y 


at K ON s EA EE (14 -80) 
WA. e UE 
则 轨迹 信息 矩阵 (14 - 63) 变 成 
S. Sy K. 
sanla Sa s. (14 -81) 
Sy ‘Ba s 
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S, =— =V 


MIT aa Inp(x,,, Žin) ] | 
e |Van a V ,[Inp(x,,,., LINE ) + 
Inp(z, | x,) + Inp(x, | x,4)]]] 


=- elY, , AA Lee Saad 11 


同样 有 
$5 = C, asa + Daun 
S, = By in a 
$4, =0 
$5 =D 


nin -1 


S, =D? 


nin 


其 中 A,., -211:2-2 ` 
式 (14 -70) 定 义 , 而 


(14 -82) 


(14 -83) 
(14 -84) 
(14 - 85) 
(14 - 86) 
(14 -87) 


B, 2,5 和 C, uua 分 别 由 式 (14 - 68), sÇ (14 - 69) 和 


Diui £- e (V, Vlnp(x, | x,4)] (14 -88) 
D? ,, À - &|V,, V, Inp(x, | x, )] (14 - 89) 
Dina = [Dil (14 -90) 
D2 À - slVV lnp(x, | x,.,)} - ei V,V.lnp(z,! x,)} (14-91) 
可 将 式 (14 -81 ) 写 为 
Åi in-2i1:n-2 B, n-21a-1 0 
J al Bea Tn Boss (14 -92) 
0 Dyin lis 
用 式 (2 -19) RERE (14 - 92) 的 逆 ， RU HERA FARA A 
A,..-ə1 n 2 Bl, yn 


J, s pa le BL: 


T 
B,. :n=2[n-1 


nln 


代入 式 (14 -77) , 则 式 (14 -93) 变 成 的 递归 , 即 
J, = Di, - DE [Ja + Drun] Dia 


nin 
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Coa, aaa, d ee. 


nln-1 


= Ds -D. tab Cin-l + Di Iin- D MM SAM ee ida D; 


nl n-l 


(14 -93) 


(14 -94) 


用 式 (14 -61 ) 初 始 化 这 一 递归 , 设 
Jo = ellV Inp( xo) ] [Va Inp(x,) ] | 


14.4.4 高 斯 加 性 噪声 的 Cramer - Rao 下界 
考虑 动态 模型 和 观测 模型 均 为 高 斯 分 布 的 情况 , 即 


x, = f. GR.) t Pa 
z, = h,(x,) +W, 
v, ~ N(0,Q) 
w, ~ N(0,R) 
如 果 初 始 分 布 是 高 斯 的 , 即 
P(X) = N (xo ;X0 P ) 
那么 则 有 


Vlnp(xo) = V, [e - SL (xo - £) TPS] (x, - 40) 1} 


= — LPT] x, - Xo) 
这 样 ,由 式 (14 -95) 以 及 [PY] "= [PZ] Jo 变 成 
Jo = E| [Po] “tx, - X4) (x, -x,) [PZ] | 


(14 -95) 


(14 -96) 
(14 -97) 
(14 - 98) 
(14 -99) 


(14 -100) 


(14 - 101) 


= [Pr] ‘el (x - Xo) (Xo -X,)"| [PZ] ' = [P7]? 


类 似 地 ,有 
F lola t a) = Ra , 
Y, Inp(z, | x,) = [Vh (x,) ]R;'w, 
这 样式 (14 -88) - 式 (14 -91) 变 成 
D'sua = sl PLAQUE 
DY, = - 寻访 CD 
Di, = - Oh etea 


Di, = Q: + e| R, E| 


F, , = [V. 天- (x,_,) ] 5 5-1(TRUE) 
H, A [V5 G9]; cmm 


(14 - 102) 


(14 - 103) 
(14 - 104) 


(14 - 105) 
(14 -106) 
(14 -107) 
(14 - 108) 
(14 - 109) 


(14 - 110) 
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总 之 ,对 于 高 斯 噪声 下 的 非 线性 模型 ,可 以 由 式 (14 -95) 5X (14 -94) ,并 
用 式 (14 -105) ~ 式 (14 - 108) 代替 式 (14 -88) ~ 式 (14 -91) ,递归 计算 
CRLB, 

在 CRLB 计算 中 会 遇 到 两 个 问题 。 第 一 个 是 式 (14 - 105) ~ 式 (14 - 108) 
Hel | 的 计算 。 在 高 斯 分 布 情况 下 ,因为 P, ,和 应 ,分 别 在 轨迹 点 zx | A x, 
进行 计算 ,可 以 用 蒙特 卡 洛 方法 来 估计 这 些 期 望 值 。 利 用 式 (14 —96) ,噪声 可 
H v, ~N(0,Q) 采 样 得 到 ,可 生成 M 组 蒙特 卡 洛 轨迹 值 |x, ,in =1,……,Nim= 
1,… ,MI 。 对 每 个 时 间 点 的 蒙特 卡 洛 轨迹 求 平均 ,从 而 得 期 望 值 的 估计 。 第 二 
个 问题 是 Q, 当 为 奇异 时 出 现 , 此 时 它 的 道 不 存在 。 对 这 种 特殊 情况 的 处 理 方 
法 超出 了 本 书 的 研究 范畴 , 感 兴 趣 的 读者 可 以 参阅 文献 [4] (也 可 以 在 文献 [2] 
的 第 686 页 找到 ) 。 


14.4.5 ” 零 过 程 噪声 的 高 斯 Cramer — Rao 下界 
当 过 程 噪声 非常 小 的 时 候 , 可 以 假设 过 程 噪声 为 零 , 以 计算 CRLB。 当 
式 (14 -96) 中 过 程 噪声 为 零 ,状态 矢量 的 演化 就 是 完全 确定 的 。 因 此 ,可 以 忽 
略 式 (14 -105) ~ 式 (14 -108) 中 的 期 望 运算 。 这 样式 (14 -94) 变 成 
J, = Q>, + Rog; RE, -OF th OP PO 
(14 - 111) 
使 用 式 (2 - 13) (B SR EIL, ERT f foo 
J, = (Q, +Ê, JF) + HR (14 - 112) 
因为 过 程 噪声 为 零 , 有 
J, = (Pr J, F, + BER; E, (14 -113) 


通过 J 的 逆 并 应 用 和 矩阵 求 逆 引 理 , 式 (14 - 113) 简 化 为 与 卡尔 曼 滤波 器 
协 方差 更 新 方程 式 (5 -30) 相 同 的 形式 , 需 计算 真 值 x, 的 雅 可 比 。 这 样 , 当 没 
有 过 程 噪声 时 , 非 线 性 滤波 器 的 CRLB 递归 与 EKF 的 协 方差 更 新 方程 形式 相 
同 ,需要 计算 真实 状态 矢量 x, 的 雅 可 比 。 


14. 4.6 ”线性 模型 的 高 斯 Cramer — Rao FR 


考虑 下 面 的 情况 ,用 线性 方程 代替 式 (14 -96) 和 式 (14 -97) 
x, = Fx, + y. (14 - 114) 
= = Hx, + Wa (14 -115) 
式 (14 -109 ) 和 式 (14 - 110) 的 雅 可 比 变 成 
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F a [W a ua =F (14 —116) 
H, A [V, x; H* ] «mur; =H (14 -117) 


因此 ,对 于 线性 高 斯 情况 , 雅 可 比 独立 于 目标 状态 ,可 以 消去 式 (14 - 105) ~ 
式 (14 -108) 中 的 期 望 值 ,得 


ins MOL (14 - 118) 
D? iaa 2-FOQ,, (14 -119) 
D?! ana =- QF (14 - 120) 
,we i (14 -121) 


把 这 些 代入 式 (14 -94) ,利用 第 2. 1.4 的 分 块 矩 阵 求 道 引 理 ,将 式 (14 -94) 
简化 为 


ps (14 - 122) 
因此 ,在 高 斯 线性 情况 下 ,CRLB 可 简化 为 线性 卡尔 曼 滤 波 器 的 更 新 状态 协 方差 
和 矩阵 ,这 说 明 在 这 种 情况 下 ,线性 卡尔 曼 滤波 器 是 最 优 的 。 


14.5 卡尔 曼 类 DIFAR 轨迹 估计 器 的 性 能 


DIFAR 案例 研究 中 用 到 的 跟踪 估计 器 包括 EKF, UKF, SSKF, GHKF 和 
MCKF。 本 节 比 较 了 每 种 跟踪 估计 器 在 DIFAR 跟踪 中 x 轴 和 y 轴 方 向 上 的 均 方 
根 误差 。 图 14 -3 ~ 图 14 -7 给 出 信和 号 信 噪 比 从 20dB 降 至 OdB , KA SdB 的 
情况 下 ,5 种 不 同 跟踪 算法 在 x 轴 和 Y 轴 方 向 上 的 均 方 根 误差 及 CRLB。 如 图 所 
示 , 当 应 用 于 DIFAR 问题 时 ,所 有 信 噪 比 下 ,不同 跟踪 算法 之 间 只 有 很 小 的 差别 。 


20dB 时 x 轴 均 方 根 
误差 Mn mile 





误差 /nmile 





20dB 时 y 轴 均 方 根 





-2 a ej -0.5 


0 0.5 l 1.5 2 
x- t/n mile 


图 14 -3 当 输 入 信号 信 噪 比 为 20dB 时 ,5 种 不 同 跟踪 估计 算法 均 方 根 误差 比较 
159 


























= 
zi 
LE 
z3 
-2 -1.5 -] -0.5 0 0.5 l L5 2 
Ë, 035 
A on 2 
EW 0.15 
&U 01 
dis 
-2 -L5 -1 -0.5 0 05 1 15 2 
x- 位 置 /n mile 

图 14 -4 当 输 入 信号 信 品 比 为 15dB 时 ,5 种 不 同 跟踪 估计 算法 均 方 根 误差 比较 
x 
Re 
RE 
Fe 
EE 
E. 4 
RE 3 
š 8 
he 
BA o 
š x 

x- 位 置 /n mile 

图 14 -5 当 输 入 信和 号 信 哄 比 为 10dB 时 ,5 种 不 同 跟踪 估计 算法 均 方 根 误 差 比 较 
x 
z 2 
c 
k 
= 
z 8 
E 
k 

x- 位 置 /n mile 
14-6 当 输 入 信号 信 品 比 为 dB 时 ,5 种 不 同 跟踪 估计 算法 均 方 根 误差 比较 
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20dB 时 x 轴 均 方 根 
jR¥ /n mile 





20dB 时 y 轴 均 方 根 
误差 /n mile 











Xx- 位 置 /n mile 


图 14 -7 当 输 入 信号 信 品 比 为 0dB 时 ,5 种 不 同 跟踪 估计 算法 均 方 根 误差 比较 


每 幅 图 中 下 端的 灰色 线条 为 CRLB。 基 于 这 些 性 能 的 度量 可 知 ,对 于 DIFAR [n] 
题 ,应 该 采用 卡尔 曼 滤波 器 ,因为 它 能 最 大 限度 地 减少 计算 载 具 。 从 这 些 数据 很 
容易 看 到 ,对 于 所 有 信 噪 比 的 输入 信号 , 当 目 标 船 舶 进入 浮标 场 接近 (0,0) 的 位 
置 时 ,滤波 器 的 跟踪 性 能 最 好 。 但 是 当 目标 离 浮标 场 很 远 时 , 随 着 与 浮标 场 中 心 
距离 的 增加 以 及 信 噪 比 降低 ,跟踪 性 能 降幅 增加 。 此 外 , 信 噪 比 低 于 SdB 时 ， 
SSKF 跟踪 器 变 得 不 稳定 ; 当 信 品 比 为 0dqB 时 ,MCKF 也 变 得 不 稳定 。 在 RMS 图 
中 不 包括 这 些 情 况 。 
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蒙特 卡 洛 方法 





第 15 € ”蒙特 卡 洛 方法 概述 
第 16 € 序 贯 重要 性 采样 粒子 滤波 器 
第 17 章 广义 蒙特 卡 洛 粒 子 滤波 器 


第 15 章 
蒙特 卡 洛 方 法 概述 


第 开 部 分 研究 了 计算 非 线性 函数 高 斯 加 权 和 矩 积 分 的 数值 方法 。 其 中 多 数 方 
法 ,都 采用 多 维 多 项 式 展开 通 近 非 线 性 函数 ,用 确定 性 矢量 点 集合 计算 高 斯 矩 积 
分 ,得 Sigma 点 卡尔 曼 滤波 器 。 第 12 章 中 给 出 了 另 一 种 方法 ,用 一 组 离散 的 随 
机 样本 来 逼近 多 维 高 斯 密度 ,而 不 是 盘 近 非 线性 函数 。 该 方法 可 以 用 非 线 性 函 
数 在 采样 点 处 的 加 权 和 计算 矩 积 分 。 当 采样 点 的 数目 增加 时 , 8 IE E 49 E A 
确 。 蒙 特 卡 洛 采样 方法 可 以 用 来 直接 求解 一 些 具 有 解析 形式 的 密度 函数 ,例如 
学 生 t 密度 . 瑞 利 密度 和 卡 方 密度 ,但 首先 要 考虑 的 问题 是 这 种 蒙特 卡 洛 积分 方 
法 是 否 适用 于 密度 未 知 或 难于 采样 的 情况 。 

本 章 首先 研究 如 何 仅仅 基于 一 组 密度 的 随机 采样 ,来 估算 密度 的 形式 。 那 
么 如 何 获得 最 初 的 采样 呢 ? 这 里 提出 的 方法 包括 多 维 直方 图 的 生成 。 

之 后 提出 了 直方 图 的 核 密 度 通 近 ,以 提供 更 平滑 的 密度 函数 估计 。 当 应 用 
于 多 峰 密 度 或 高 度 非 高 斯 密度 的 可 视 化 \ 以 及 在 未 提供 任何 有 意义 的 信息 情况 
下 的 矩 估 计时 , 核 估 计 的 实用 性 变 得 非常 明显 。 

Richardson 等 “给 出 实例 ,密度 在 两 个 维度 上 是 多 峰 的 ,和 矩 不 代表 任何 模式 
的 峰值 (通常 会 有 这 样 一 个 错误 的 印象 ,峰值 可 能 对 应 感 兴趣 的 目标 的 位 置 ) 。 
在 下 一 章 讨论 重 采样 方法 时 ,还 将 发 现 核 密度 估计 的 作用 。 在 讨论 完 核 密度 估 
计 方法 之 后 ,继续 回 到 如 何 由 未 知 密度 通过 引入 重要 性 采样 概念 来 生成 样本 。 
最 后 一 节 对 本 章 的 内 容 进 行 了 概括 与 总 结 。 


15.1 通过 蒙特 卡 洛 样本 集 估计 密度 


15.1.1 由 二 维 高 斯 混合 密度 生成 样本 


本 节 将 讨论 一 些 仅仅 基于 密度 的 采样 ,来 估算 密度 形式 的 方法 。 通 常 从 密 
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度 的 采样 (数据 ) 来 推断 未 知 密度 ,因此 必须 采用 一 种 非 参数 方法 。 这 种 非 参 数 
密度 估计 方法 可 以 提供 视觉 线索 ,揭示 偏 态 分 布 或 数据 中 存在 多 个 模式 ,可 以 进 
行 观 测 值 的 重要 物理 解释 。 例 如 ,考虑 双 模 高 斯 混合 密度 
p(x) = w N(x, 3X,,3,) + w, N(x,;X,,E,) (15-1) 
这 里 w +o, =1。 由 这 样 一 个 混合 密度 可 以 很 容易 进行 采样 ,如 果 所 需 的 样本 
总 数 为 入 ,从 N(x 3) X ) 中 选取 o IN 个 采样 ,从 N(x,,x*,,3,) 中 选取 w,N 个 
采样 。 图 15 -1 中 N=1000 且 (wi,w,) =(0.3,0.7)。 同 时 ,设置 
X =(-5,-8) 
X, u(T,7) 
100 -5 
"des 4 


E. 8 
z =|; 64 
15.1.2 利用 多 维 直方 图 拟 合 密度 


本 节 提 出 一 种 可 视 化 的 概率 密度 函数 ,注意 落 在 给 定 区 域 的 样本 概率 由 落 
在 该 区 域 的 样本 的 归 一 化 密度 得 。 这 样 可 将 归 一 化 的 二 维 直方 图 作为 首选 的 估 
计 和 可 视 化 方法 ,因为 它 实 际 上 度量 的 是 特定 的 二 维 区 间 内 观测 到 样本 的 概率 。 
可 将 多 维 直方 图 密度 估计 定义 为 


X, 


P(x) = NE (15 -2) 
式 中 :为 落 在 超 体 积 条 (bin)4x 内 的 样本 数目 。 例 如 ,对 于 一 个 二 维 密度 4 = 
(xi. —%;) (yia 一 ye) ,如 果 所 有 的 条 大 小 相同 ,可 以 令 h = (5; -xi) = E es 
y), Vi,k H. 


p(x) = = (15 -3) 


这 样 , 对 于 一 般 n, - 维 直 方 图 ,有 

P(x) = vum (15-4) 
AP iv, 为 落 在 第 i 个 超 体 积 条 内 的 样本 数目 。 后 面 的 直方 图 中 只 有 一 个 自由 
参数 一 一 条 的 宽度 人 。 直 方 图 可 以 渐 近 逼近 任意 连续 密度 ,因此 可 以 称 之 为 非 
参数 估计 。 带 宽 h 的 选取 是 近期 研究 的 重点 ,此 处 不 做 详细 介绍 ,可 参阅 
Scott!” RHA SABA TCHR. Scot 给 出 如 何 使 用 均 方 误差 求 和 (IMSE) 以 及 误 
差 平方 和 的 均值 ( MISE) ,基于 样本 数据 估计 最 优 带宽 。 
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可 以 从 MATLAB 中 心 “ 下 载 生成 并 绘制 二 维 直 方 图 的 MATLAB 程序 , 注 
意 ,这 些 子 程序 必须 适合 特定 的 数据 集 ,在 编写 时 是 非 规 范 化 的 。 图 15 -2 中 给 
出 了 由 图 15 -1 中 数据 生成 的 二 维 规范 化 直方 图 。 根 据 数 据 的 最 大 范围 ,在 每 
个 维度 上 选取 合适 带宽 ,产生 99 个 条 。 











-30 


图 15 -1 二 维 高 斯 混合 密度 的 采样 





10 20 3 


15-2 ”基于 高 斯 混合 分 布 采样 的 二 维 直方 图 


15.1.3 核 密度 估算 


式 (15 -3) 给 出 了 落 在 中 心 为 xe R° 的 给 定 二 维 条 内 样本 的 概率 估计 。 在 
条 区 域 为 零 的 极限 情况 下 ,每 一 个 条 中 最 多 包含 一 个 样本 。 这 时 ,可 以 将 样本 密 
度 函 数 写 成 


p.(x) = Lae - 2) (15 -5) 
可 以 将 其 看 作 离 散 采 样 的 密度 ,因为 
Ex 
Je. Gods xd pols aie a (15 -6) 
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图 15 -2 中 的 直方 图 是 块 状 的 ,在 每 个 条 的 边界 处 不 连续 。 此 外 , 它 的 导数 
只 在 条 的 边界 处 非 零 ,在 其 他 位 置 处 处 为 零 。 这 个 困难 可 以 通过 对 p,(x) 用 一 
个 平滑 多 尺度 核 函 数 K, (x) 进行 卷 积 ,以 代替 直方 图 估计 器 来 解决 , 即 


p(x) = (p,* K,)(x) A [p.(u) Ka(x - u)du 
R"x 


s {wae = x?) Ky(x — u)du = IY Ka(x = x) 
(15 -7) 
根据 核 的 定义 ,多 尺度 核定 义 如 下 
Kn(t) À rZ KE t) (15 -8) 
ix B K( u) ANERER PRÉC, ERAF 
f Ku) du =1 (15 -9) 
à. 
[v K(u)du = 0 j= 1, -1 (15 -10) 
R” 
fu K(u)du #0 (15 - 11) 
R^ 
K(u) »0,Vu (15 -12) 


Ak ARR RK. ERE H HICK AK PROC EE HE ( sk F) SAW PR 
BOGS th Jr 35 kB BE DL, Fukunaga! 和 Silverman! 的 专题 论文 是 非常 好 的 
介绍 核 密度 估计 的 文献 。 


15.1.3.1 单 变量 核 密 度 估计 


首先 考虑 单 变 量 情况 ,文献 [7] 中 研究 了 多 种 单 变 量 核 函 数 , 表 15 - 1 中 列 
举 了 其 中 最 为 常用 的 。 表 15 - 1 中 指标 函数 了 的 定义 如 下 


| l -l<u=<1 
ult) = | (15 -13) 
0 其 他 
为 表明 核 图 数 在 一 维 密度 估计 中 作用 ,考虑 12 个 一 维 数据 点 的 小 集合 
| -7.5, -6, -3, -2, -1.5, -1, -0.3,0,3.5,4,6,8| 。 设 标记 的 原始 数据 点 
的 数量 为 Nu ,将 表 15 -1 PRIA 15 -7) ,得 核 密 度 估计 为 








p(x) -49 K,(x - 2) = Yd K (===) 
ad $ 4 i1rx-a9p 
ihel) ase 
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表 15 -1 常用 2 阶 单 变 量 核 函 数 


(1-lul)l(lul<1) 


DE 
l l 
zl 


很 容易 看 出 ,带宽 参数 h 正好 是 高 斯 密度 的 标准 偏差 。 还 可 以 看 到 ,密度 估计 
P(x) 只 不 过 是 一 个 高 斯 混合 密度 ,对 于 每 个 单独 以 初始 采样 点 为 中 心 高 斯 密度 
取 相 同 的 权 值 。 

将 单个 采样 点 核 函 数 定义 为 


K,(x) = xp( - 





l 
2 





HESET) orem 
(15 -15) 
图 15 -3 绘制 了 两 个 不 同 h 值 的 完整 估计 核 密度 和 单个 的 核 函数 ,其 中 x 的 取 
值 范 围 -15$ 三 x 达 15。 
l 由 图 15 -3 可 见 ,h 值 的 选取 非常 重要 。 在 第 一 个 图 中 , 取 h=0.9, 密 度 是 平滑 
的 ,并 且 很 好 地 逼近 了 采样 点 密度 ;第 二 个 图 中 , 取 岂 =0.3 ,密度 估计 几乎 在 每 个 采样 
点 都 有 一 个 峰值 ,很 不 平滑 。 有 评论 认为 值 的 选取 比值 选取 重要 得 多 ” 。 
假设 原始 样本 集 来 自 于 高 斯 分 布 ( 当然 实际 情况 未 必 如 此 ) , 即 假设 真实 密 
度 可 以 由 N(x;%,,o?) 近 似 ,这 里 x, Al o? 分 别 为 样本 均值 和 方差 。 如 果 进 行 如 
下 变换 
(15 -16) 
得 
N («3x,,0°) a = ME ee 5 1) (15 -17) 
I c, T, T,’ 0,’ 


这 样式 (15 -15) 变 成 


KG) = ng, NC x? 250) < l 


NM E 
时) 

(15 -18) 
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h- 0.3 


图 15 -3 稀疏 采样 数据 的 高 斯 核 密度 估计 





进行 仿 射 变换 
x =x" +o he (15 -19) 
对 式 (15 -18 ) 进 行 修正 ,得 
KG) = Y "zen E le) (15 -20) 
这 样式 (15 -14) 简 化 为 
p(x) = X X N Ces, 1) (15 -21) 


这 样 可 得 一 组 新 的 样本 限 |x”,j = 1,…, Nw | ,这 里 N... 可 以 远 远 大 于 


Nu, 可 以 根据 表 15 -2 中 的 步骤 得 p(x) 值 。 
表 15 -2 生成 一 维 核 函 数 采样 点 


(1) 将 区 间 (0,1] 细 分 为 Nu] qi=(i-1)/Naa iml, Nul 


(2) 由 一 致 性 分 布 生成 N... AREA: u; ~u(0,1] j=1,,N 


REWE q; <u<g,,1 的 i 值 


c? ~N (c;0,1) 
(5) ÆR x 的 新 样本 $9 zy 
(6) 对 于 j=1,… ,Now ,重复 步骤 (2) ~ (5) 


因为 这 里 利用 了 核 估计 方法 与 混合 密度 方法 得 的 权重 相同 这 一 事实 ,正则 
化 过 程 变 得 非常 简单 。 下 面 将 给 出 推广 到 包括 不 相等 权 值 的 多 变量 情况 。 注 
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意 , 高 密度 采样 区 域 会 选 到 更 多 的 重 采 样 值 。 
应 该 指出 的 是 ,采用 高 斯 分 布 之 外 的 核 函 数 时 , 核 密度 估计 的 重 采样 有 时 困 


难得 多 。 例 如 ,由 Epanechnikov 核 函 数 生成 样本 包括 生成 VBu 步骤 ,这 里 B 服从 
参数 为 (1/2,2) 的 BB 分 布 u 为 均匀 分 布 的 采样 ,参见 上 面 步 骤 (2)"。 
关于 多 维 核 方法 的 进一步 讨论 可 以 参阅 文献 [9 -12]。 


15.1.3.2 多 变量 核 密度 估计 
对 于 多 参数 高 斯 核 函 数 , 由 式 (15 -18) 可 得 密度 估计 器 为 


n l l ; 
p(x) = 2, N (am)? | SH 7 xg esp] fe - a)" 


[ZH]"(x -x'?)] (15 -22) 


很 显然 ,多 参数 单个 核 函 数 为 高 斯 密度 N(x;x'”,2 及 ) 除 以 No 
考虑 n, =2 的 二 维 情况 ,其 中 


hi hy 
H = | | (15 -23) 
hy hi 
在 这 种 情况 下 ,可 以 有 3 个 不 同 的 平滑 参数 :一 个 用 于 维度 1; 一 个 用 于 维度 2; 
还 有 一 个 用 于 两 个 维度 之 间 的 相关 性 。 但 与 一 维 情况 相 比 ,选择 “最 优 ”的 平滑 
参数 更 为 复杂 。 假 设 数据 密度 为 高 斯 分 布 , 即 x ~ MV (x;x 3) ,可 以 由 该 数据 生 
成 一 个 样本 协 方差 矩阵 ,对 其 进行 矩阵 分 解 X, = DD! ,可 利用 如 下 变换 实现 
数据 规范 化 , 即 
x = D;'x (15 —24) 

得 

x ~| 也 1- N(D;'x;D;!x,,I) (15 -25) 
由 于 变换 后 的 数据 有 一 个 单位 对 角 采 样 协 方差 矩阵 ,可 将 H 的 非 对 角 线 项 设 为 
零 , 并 且 设 h=h =h, 因 此 有 I1H1” = 及 。 这 样 ,对 于 二 维 情况 , 式 (15 -22) 
变 成 


N 
di A. l _ jn- _ n2. GO3Tgr-1 
(D;'x - D;'x®) | (15 -26) 


如 果 最 后 实施 的 仿 射 变换 为 
x = x +Dhe (15 -27) 
171 


那么 式 (15 -26) 简化 为 
p(x) = > a N(e:0,D) (15 -28) 
将 以 上 推导 推广 至 2 维 以 上 非常 简单 ,这 里 作为 练习 留 给 读者 。 

可 以 从 MATLAB 中 心 [12] 下载 二 维 高 斯 核 密度 估计 子 程序 ,其 中 算法 经 过 
改进 后 ,可 实现 由 图 15 -1 中 的 数据 生成 平滑 的 密度 估计 。 图 15 -4 给 出 估计 
pdf 的 可 视 化 结果 。 

与 一 维 情况 类 似 , 按 照 表 15 -3 中 的 步骤 ,很 容易 得 到 多 维 高 斯 核 孙 数 
采样 。 

30 


20 


-30 —20 -10 0 10 20 30 


15-4 由 密度 函数 随机 采样 生成 的 高 斯 核 密度 估计 的 可 视 化 


表 15 -3 生成 多 维 核 采样 
(1) 将 区 间 (0,1] 细 分 为 Wu 个 增 其 q, =(i-1)/Noy i=1,. Nad +1 


(i Re ERNE 
(3) 对 于 uw, 决定 其 合适 的 q, 区 间 找到 满足 qi <u;<q,,, 9 i {ii 


(6) AT j 21, Nuen ,重复 步 又 (2) ~ (5) x) =x 4D cO 
即 , 由 一 组 随机 样本 1xz”… i2 1, N.a |. PET AK ES x, 
j= l $2 oe | ;这 里 ‘a 可 以 远 远大 于 No 


15.2 ”重要 性 采样 的 基本 概念 


由 本 书 第 了 部 分 可 知 ,几乎 所 有 的 非 线 性 系统 高 斯 估计 (跟踪 ) 方 法 都 涉及 
某 种 密度 加 权 矩 积分 的 数值 计算 。 其 中 一 些 数值 方法 包括 函数 在 特定 确定 性 
Sigma 点 取 值 的 加 权 和 。 
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如 果 加 权 密度 是 多 参数 AE TP MUT sk AR AY RAAE EE 9 f, 
一 般 不 能 使 用 确定 性 数值 方法 。 蒙 特 卡 洛 方法 在 一 般 密 度 加 权 积 分 方面 最 近 
( 近 30 年 来 ) 的 研究 进展 ,已 经 广泛 地 应 用 于 估计 和 跟踪 当中 。 这 些 方法 中 最 
具 影 响 力 的 是 独立 重要 性 抽样 。 

读者 可 以 参考 Robert, Casella"! 和 Doucet 5&5! 的 书籍 ,还 有 Doucet05] 和 
Liu Chen 的 论文 ,及 其 包含 的 参考 文献 。 其 中 有 一 般 蒙 特 卡 洛 积分 方法 和 序 
贯 重 要 性 采样 方法 的 起 源 和 基本 发 展 的 完整 阐述 。 

第 12 章 用 高 斯 密度 生成 蒙特 卡 洛 采 样 来 求解 高 斯 密度 加 权 积分 ,将 积分 简 
化 为 函数 在 采样 点 取 值 的 加 权 和 。 对 于 未 知 密度 或 非 高 斯 密度 ,由 该 密度 生成 
蒙特 卡 洛 样本 过 程 可 以 用 重要 性 采样 来 高 效 实现 。 在 重要 性 采样 中 ,由 已 知 重 
要 性 密度 4(x) 生 成 一 组 独立 的 蒙特 卡 洛 采样 1z'… ,…,x' | ,然后 进行 被 积 函 
数 采样 点 取 值 加 权 和 (平均 ) 计 算 。g(x) 的 取 值 自由 度 很 大 ,最 好 选择 所 生成 的 
采样 值 落 在 对 于 积分 值 很 重要 区 域 ,不 在 这 个 区 域 的 被 积 函 数 是 可 以 忽略 不 
计 的 。 

重要 性 采样 的 概念 ' 源 自 于 如 下 形式 的 积分 , 即 


F(x) = [f(x)p(x) dx (15 -29) 


首先 考虑 p(x) Aad i B diro. — fal iE EAT B3 yi EE H REA M (zi 
x,P,) ERARE x’ im 1, NT ,用 p(x) 的 离散 采样 代替 函数 本 身 , 即 


p(x) = + yal -x") = Y d(x - x”) (15 -30) 
i i21 i=] 
这 里 o, =1/N, Vis 式 (15 -29) 的 积分 变 成 
f(x) = [foo Y wó(x - x )dx = Y w f(x) (15 -31) 


Rw, 表示 x 相对 于 样本 均值 (总 和 ) 的 重要 性 。 因 为 可 以 直接 从 已 知 密度 
中 进行 采样 ,在 高 斯 密度 情况 下 ,所 有 样本 相对 于 样本 均值 总 和 的 重要 性 相同 ， 
因此 w; =1XN。 这 里 权 值 o, 不 代表 x? 的 概率 ,只 是 表示 用 加 权 和 通 近 和 矩 积分 
时 x'” 的 重要 性 。 文 献 [13 ] 中 指出 , 当 N— oe 时 , 式 (15 -31) 收 敛 于 精确 解 。 

假设 p(x) 的 PDF 未 知 或 采样 困难 ,再 假设 有 另 一 个 已 知 或 容易 采样 的 
PDFq(x) , 称 之 为 重要 性 密度 ,后 面 将 解释 这 样 命名 的 原因 。 举 例 说 明 ,p(x) 为 
未 知 密度 ,g(x) 为 高 斯 密度 。 假 设 g(x) 的 支撑 大 于 或 等 于 p(x) 的 支撑 ,这 样 在 
每 个 x 值 处 ,p(x) 与 q(x) 成 比例 。 由 于 p(x) 是 一 个 规范 化 的 PDF g(x) HAE 
p(X) 的 比例 缩放 ,对 于 每 个 x, 有 不 同 的 尺度 因子 。 对 于 任意 两 个 具有 相同 支撑 
的 PDF ,这 是 绝对 正确 的 。 现 在 ,可 以 将 比例 或 权重 因子 定义 为 

p(x) 


adipi" 


(15 -32) 
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必要 条 件 是 q(x) 的 支撑 大 于 或 等 于 P(r) 的 支撑 。 因 此 ,在 许多 情况 下 , 当 z 的 
PUEH -© <x;<% ,i=1,…,n, 时 ,g(x) 最 好 的 选择 是 多 维 高 斯 分 布 。 

图 15 -5 给 出 了 一 维 的 例子 ,其 中 p(x) (上面 图 中 的 实 线 ) 为 多 峰 的 ,q(x) 
(上 面 图 中 的 虚线 ) 为 比 p(x) 更 宽 的 高 斯 PDF ,在 图 (15 -5) 中 给 出 了 w(x) 生 
成 示例 。 


o(x)-p(x)/q(x) 
° — 

Oun — Ud ia 
| 


图 15$ -5 w(x) 生 成 示例 


必须 强调 ,w(x) 不 是 密度 函数 ,而 是 两 个 密度 的 非 规范 化 比值 。 即 使 在 规 
范 化 后 ,w(x) 也 不 能 取代 PCr) 。 在 使 用 zx 的 最 大 后 验 估计 (MAP) 时 ,这 一 点 很 
重要 。 如 果 g(x) 的 最 大 值 与 p(x) 的 最 大 值 不 一 致 ,那么 w(x) 的 最 大 值 就 不 是 
P(x*) 的 最 大 后 验 估计 。 

假设 g(x) 为 高 斯 分 布 ,有 g(x) =N (xiu, )。 根 据 g(x) 的 采样 值 ,x" 可 
以 求 得 x; 的 概率 p(x) , 即 
l 





q(x = x) = aor - a = x) -u)°} (15 -33) 
从 而 有 
w(x =x) = — (15 -34) 
q(x = x 


对 于 已 知 分 布 q(x) 的 多 维 采 样 |x” ,i= 1, INT ,采用 某 种 有 待 确定 的 方法 来 
为 每 个 样本 生成 co, F p(x) 的 离散 值 取代 函数 本 身 , 即 
p(x) = Y wd(x ~x") (15 -35) 
这 样 积分 可 以 被 重 写 为 
f(x) = Jrz)p(z)dr = [fG)w()gGo dx 
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= [fæ Y wate - -x)dx = Su f(x?) (15 -36) 


上 式 与 式 (15 -31) 的 形式 完全 相同 ,但 权重 o, 的 取 值 不 再 统一 等 于 1/N。 由 于 
权重 是 式 (15 -35 ) 离散 密度 逼近 定义 的 一 部 分 ,很 显然 必须 对 权重 进行 规范 
化 ,使 得 Xo, =1。 

考虑 一 阶 马 尔 可 夫 过 程 x, ,其 估计 和 矩 由 条 件 后 验 密度 px, IZ, ) 确定 ,其 中 
px, ) 为 得 任何 观测 值 之 前 的 初始 后 验 概率 (参见 3.3)。 给 定 任意 后 验 PDF, Hi 
的 非 线性 函数 通用 表示 形式 为 


I(x,) = |g(x.)p(z, | z,,,)dx, (15 -37) 


这 里 g(x, ) 是 一 个 函数 ,可 以 表示 任意 矩 生 成 函数 。 例 如 , 如果 要 求 得 函数 

(xz,) 的 均值 ,那么 由 8g(r,) =f(x,) 可 导出 T(x,) =f(x,)。 如 果 要 计算 f(x,) 

的 协 方差 , 令 g(x,) = [f(x,) -f(x,)] [f(x,) -f(x,)]", 可 得 I(x,) =P", 
(BLUE p(x, |Z), ) 未 知 或 采样 困难 ,但 是 有 一 个 很 容易 的 采样 重要 性 密度 

q(x,12,,) ,与 p(x,1z,,) 的 支撑 相同 ,其 最 大 值 与 p(x,1z1, ) 在 大 致 相同 的 位 

置 , 式 (15 -37) 可 写 为 

p(x, l z ) 


IG.) = Je Teg Ie! Fi) Me 
= fate, en, ral, | z,, dx, (15 -38) 
X BUS PAK w(x,) 定义 为 
p(x, | Zia) 
w(x,) & qx, z ) (15 -39) 


Xt px, 12,., ) 使 用 式 (3 -23 ) 贝 叶 斯 法 则 3€ (15 -39 ) 变 为 
_ PG, | x,)p(x, | 2,4) l 


wie, p(z,!z,,,) gix ie, calla. 
注意 p(z,1z1., 1) 恰好 为 正则 项 ,得 
| 
eG) < PETE E) err 


q(x, | Zin) 
这 里 < 表示 成 比例 。 根 据 似 然 函数 ,p(z,1x, )、 先 验 PDFp (x,1z.,,_1) 和 重要 性 
PDFg(x,1z1.,), 式 (15 -41) 给 出 一 般 加 权 函 数 。 

如 果 选 择 |x。 ,i =1,…,NN,| 样 本 集合 ,可 通过 狄 拉克 5 函数 组 ( 见 式 (15 -5) ) , 
18 qx, 12,,, ) fh 3T 7 


q(x, | Zin) - iYsc. -x. (15 -42) 
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式 (15 -38) 变 成 


Hx.) = fer wl) ac Eea, x de, = ex) 


(15 -43) 
Qi A (xi?) 
HX (15 -39) ,得 
(i) | 
w, = co ' = (15 -44) 
或 者 ,利用 式 (15 241) ,得 一 组 非 规范 化 权 值 , 即 
(i) (i) | 
zO _ plz | x, )p(x, |z...) (15 -45) 


q(x, | z,,) 
由 于 将 式 (15 -41) 中 的 成 比例 替换 为 相等 ,因此 权 值 是 非 规范 化 的 ,可 以 通过 
令 其 总 和 为 1 来 归 一 化 权重 , 即 


~(i) 
一 (15 -46) 
1/N, ` wi” 
i=l 
这 样式 (15 -43 ) 变 成 
N, 
I(x,) = Jwi g) (15 -47) 
i=l 


这 里 再 着 重 强 调 一 次 ,w,” 不 是 x. 的 概率 ,而 是 x. 在 离散 和 矩 积 分 和 中 的 重 
要 度 。 


15.3 总结 


本 章 提出 了 两 个 主要 概念 :一 个 是 基于 一 组 密度 函数 采样 集 ,来 表示 该 密度 
天 数 的 方法 ; 另 一 个 是 函数 数值 积分 中 ,由 任意 PDF 给 定 权 重 的 函数 的 蒙特 卡 
洛 重要 性 采样 的 概念 。 目 前 ,仅仅 基于 这 些 结果 ,还 未 能 提出 一 种 估计 和 跟踪 的 
实用 方法 ,还 需要 进行 进一步 的 改进 。 

基于 顺序 生成 的 蒙特 卡 洛 点 和 权重 的 递归 集合 |xz。 m; ,i=1,…,N,,n = 
LV NY ,所 得 的 跟踪 滤波 器 被 称 为 序 贯 蒙特 卡 洛 (Sequential Monte Carlo, SMC) 
方法 。 与 现 有 的 其 他 技术 相 比 ,SMC 方法 具有 诸多 优点 :QD 它 允 许 在 一 般 状 态 
空间 模型 ,包括 非 线性 非 高 斯 条 件 下 的 后 验 分 布 推理 ,因此 利用 SMC 方法 可 以 
计算 各 种 和 矩 .分 位 数 和 最 大 后 验 密 度 区 域 ,而 高 斯 卡尔 曼 滤波 器 仅 能 进行 一 阶 、 
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—Bri i38 ir 5 QB EG XE A ae BJ K AS 25 [8] PEAY, SMC 方法 可 结合 状态 空间 约 
束 ,如 用 限制 状态 空间 的 范围 来 表示 物理 限制 ,例如 速度 约束 或 硬 边界 。 

下 一 章 将 介绍 一 个 与 SMC 相 比 约束 更 大 的 方法 ,用 顺序 的 方式 为 每 个 采样 
点 生成 一 个 权重 ,得 序 贯 重 要 性 采样 (SIS ) 粒子 滤波 器 。 第 17 章 将 介绍 一 种 约 
束 较 小 的 方法 ,但 是 在 每 次 迭代 中 ,需要 重新 计算 权重 函数 。 
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第 16 章 
序 贯 重 要 性 采样 粒子 滤波 器 


16.1 序 贯 重要 性 采样 的 基本 概念 


在 第 卫 部 分 提出 的 所 有 高 斯 卡尔 曼 滤 波 估 计 器 中 ,i, 时 刻 的 估计 值 以 一 种 
解析 方式 依赖 于 t,_, 时 刻 的 估计 值 。 本 章 要 找到 一 个 类 似 的 递归 公式 , 当 噪 声 
密度 为 非 高 斯 时 ,用 重要 性 采样 方法 进行 估计 。 

基于 权 值 序列 估计 的 递归 估计 方法 被 称 为 序 贯 重要 性 采样 (SIS) 粒 子 滤波 
器 。 利 用 式 (3 -24) 的 Chapman - Kolmogorov 定理 ,可 将 式 (15 -41) 中 的 p(x, | 
Zien-1 ) 和 q(x, Izi., ) 进 一 步 展 开 ,这 样式 ( 15 -41 ) 变 成 


p (z, | x,) [p(x, | X44 E ul )p (x, | Zia- ) dx,_, 
— n N (16-1) 
Jas. | X, Zin) (Eni | sS sssi )dx,_, 


在 分 母 中 包含 94(x, ,1z, ) 而 不 是 g(x,_11z,., ) 的 原因 在 于 :x, -不 能 以 未 来 
的 观测 作为 条 件 。 假 定 状态 演化 方程 是 独立 于 观测 值 的 一 阶 马尔 可 夫 过 程 , 令 
p(x, Ix, i "2Z1:n-1 )—p(x, |x, _, ) ,得 


w(x,) o 


pz, | x.) [p(x, | x, px | zs A 
fax. | X, ,Z,,)9 (X, | Zia )dx, , 

对 于 马尔 可 夫 过 程 , 可 以 进行 如 下 假设 , 即 

q(x, | &,., Ë...) = g(x, | x,.,,2,) (16 -3) 


这 样 ,重要 性 密度 仅 取决 于 xx M z, 
假设 有 一 组 由 9(x,，,1z,,，,) 得 到 (第 15 章 由 g(x,1z., ) 生 成 样本 ) 随 机 样 
AR ixi? i=l, N} BD x, ~g(x,_11z1.,-1)1 ,这 样式 (16 -2) 变 成 
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(16 -2) 


w(x,) cc 


m pinizxjnir,Ux Mna...) 


b rU PC OPI, dica 
根据 式 (15 -39) 所 给 出 的 w(x,) 的 定义 ,得 
wy Rus PUS |e, pla, | al, _ 
IE ass, 


得 如 下 的 后 验 分 布 表达 形式 , 即 
N, f | a 
p(x, | Z...) 一 >, ws ar Prac | x, )q(x, | Zia) (16 -6) 


H q(x, 1x12, z. EREE Ex? ,i=1,…,NN,| , 即 





x, = q(x, | x x.) (16 -7) 
a(x, 1 Zn) = (x, xD?) (16 -8) 
得 
N ~i) (i) (i) 
z waa P(X, | Xni) (i) o. 
p(x, | K = > N. q(x | x) ay" x, )ó(x, s ) 
(16 -9) 
用 下 式 对 wN, 进行 规范 化 , 即 
Ni) 
ai Be (16 - 10) 
> wa- /N, 
得 
P(X, l z...) = Y uix, - x) (16-11) 
序 贯 重 要 性 递归 权 值 更 新 方程 为 
(i) (ü 
w 2 wid Pe | Tet? p(z, | x) (16 -12) 


V* aut" 1-5 n) 
回 到 式 (15 -37) 给 出 的 一 般 矩 方程 ,zx, 关于 p(xz,1z,) 的 前 两 阶 矩 可 以 由 下 式 
求 得 , 即 


N, 
x A e|x, | E. <= fep, | g ,) dx, = fx. Y wlx, - xi?) 
i R I R =l 


N, 
=F wx? (16 -13) 
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类 似 地 有 
N, 
PY A s| (x, -%,)(x, -¥,)"1 z = Swi? (xp — $,) (z -£) 
° iel 


(16-14) 


序 贯 重要 性 采样 粒子 滤波 器 的 步骤 非常 简单 ,如 表 16 - 1 所 列 。 
表 16 -1 一 般 序 贯 重要 性 采样 粒子 滤波 器 


L 滤波 器 初始 化 xj) ~ q(xo) 
(1) 状态 矢量 样本 初始 化 ; 
(2) 权 值 初始 化 


2. 序 贯 重要 性 采样 到 
(1) 抽取 新 样本 ; "uu NT ) pz, | x, )P(xn | x06) 
(2) 生成 非 规范 重要 性 权 值 ; ^ ii q(x | xD z.) 
(3) 权 值 的 规范 化 ; 
(4) 时 间 递 增 ,返回 1 


3. 如 需要 ,计算 和 矩 


(1) 一 阶 矩 :状态 矢量 均值 ; 8 
(2) 二 阶 矩 : 协 方差 P” = yen (x — 2.) (xf) _ x.) 





在 序 贯 重要 性 采样 粒子 滤波 过 程 中 ,包含 几 个 重要 的 实际 问题 。 第 一 个 问 
题 出 现在 序 贯 重要 性 采样 的 第 2 步 ,需要 似 然 函 数 p(z,1x, ) 、 预 测 密度 px, | 
x, 4, 和 重要 性 密度 q(x, 1x, -yz) 解 析 表 达 式 。 似 然 函 数 是 基于 观测 噪声 密度 
的 ,通常 假定 为 高 斯 密度 ,然而 当 观 测 方程 为 高 度 非 线性 时 ,这 种 假设 就 不 成 立 ， 
如 第 4. 4 节 给 出 的 DIFAR 浮标 案例 中 的 情况 ;在 动态 转移 方程 中 ,预测 密度 是 
基于 噪声 的 ,可 以 是 高 斯 , 非 高 斯 或 者 完全 未 知 的 ;重要 性 密度 通常 为 某 种 已 知 
的 分 析 密 度 ,可 以 很 容易 地 进行 采样 。 但 要 注意 ,q(x,1x,_1,z,) 是 以 当前 的 观 
测 值 为 条 件 的 ,因此 在 非 线性 高 斯 卡尔 曼 滤波 器 的 输出 中 ,重要 性 密度 通常 被 当 
作 后 验 密度 ,应 用 于 每 个 粒子 当中 ,其 中 更 新 方程 利用 最 近 的 观测 值 。 序 贯 重要 
性 采样 粒子 滤波 器 的 这 三 种 密度 非常 重要 ,必须 认真 选取 。 

序 贯 重要 性 采样 粒子 滤波 器 的 第 二 个 问题 是 粒子 的 发 散 , 注 意 只 有 在 初始 
化 步骤 才 生 成 粒子 。 尽 管 第 2 步 看 起 来 是 一 个 重 采 样 步骤 ,而 实际 上 ,通常 使 用 
非 线性 高 斯 卡尔 曼 滤波 器 作为 重要 性 密度 ,对 每 个 粒子 分 别处 理 。 在 这 种 情况 
下 ,原来 的 粒子 只 是 通过 了 滤波 器 而 没有 进行 重 采 样 。 因 此 ,最 初 的 粒子 在 每 次 
迭代 中 传播 ,在 初始 化 步骤 生成 粒子 后 ,没有 新 粒子 生成 。 因 此 ,这 类 蒙特 卡 洛 
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估计 方法 被 称 为 粒子 滤波 器 ,因为 粒子 的 轨迹 代表 通过 状态 空间 的 时 间 路 径 。 
生成 粒子 的 过 程 是 由 某 些 初始 重要 性 密度 进行 采样 的 过 程 , 之 后 粒子 就 在 序 贯 
重要 性 采样 过 程 中 一 次 次 传播 ,每 一 次 欠 代 都 会 加 入 噪声 。 这 导致 基于 离散 后 
验 密 度 的 粒子 传播 与 真实 后 验 密度 的 粒子 传播 之 间 存 在 偏差 , 除 少 数 主要 的 粒 
子 之 外 ,多 数 粒 子 的 权重 降 为 零 。 这 就 是 通常 所 说 的 SIS 退化 问题 。 在 每 次 粒 
子 传播 后 ,用 所 有 粒子 的 权重 来 进行 矩 估 计 , 但 权 值 (重要 性 ) 足 够 大 ,这 样 在 离 
散 求 和 中 起 作用 的 粒子 越 来 越 少 , 增 大 了 矩 估 计 的 方差 。 为 降低 这 种 增长 对 估 
计 方 差 所 造成 的 影响 ,提出 了 很 多 SIS 粒子 滤波 方法 。 

还 应 指出 的 是 ,重要 性 密度 的 初始 化 g(xo ) 甚 至 不 需要 与 SIS 第 1 步 中 用 到 
的 重要 性 密度 g(x,1x, -1 ,z,) 相 同 。 通 常 希望 初始 化 密度 q (x ) 尽 可 能 无 信息 ， 
以 包含 后 面 所 有 可 能 区 域 的 蒙特 卡 洛 采样 。 但 通过 最 新 的 观测 值 z,, 可 以 使 
g(x,1xX,-1,z,) 高 度 局 域 化 。 


16.2 序 贯 重要 性 采样 粒子 滤波 器 的 重 采 样 和 正则 化 


SIS 样本 退化 问题 可 以 通过 重 采样 过 程 来 缓解 。 重 采样 的 基本 思想 是 消除 
低 权 重 的 状态 空间 粒子 ,复制 高 权重 粒子 来 替换 掉 低 权 重 粒 子 。 在 这 一 过 程 中 ， 
把 权重 看 成 是 一 维 离散 概率 分 布 ,用 与 累积 分 布 权 重 的 逆 相关 联 的 逆 变 换 方 
法 "来 对 粒子 进行 重 采样 。 

然而 ,这 将 造成 新 的 问题 一 一 由 于 生成 的 样本 集合 中 包含 给 定 权 重 的 多 个 
重复 粒子 ,会 失去 粒子 的 多 样 性 。 有 文献 将 这 种 问题 称 为 样本 贫 化 。 在 严重 的 
情况 下 ,所 有 的 粒子 都 迁移 到 一 个 采样 点 。 为 了 矫正 由 重 采 样 导 致 的 样本 贫 化 ， 
在 每 次 重 采 样 过 程 之 后 ,可 以 用 粒子 密度 的 核 密度 估计 来 对 粒子 进行 重 采 样 。 
在 这 一 过 程 中 ,每 个 新 粒子 (蒙特 卡 洛 样本 ) 都 是 由 重 采样 粒子 中 基于 均匀 分 布 
选 出 的 ,并 基于 局 部 核 函 数 对 采样 点 进行 微小 移动 。 这 个 过 程 往往 集中 在 概率 
最 高 区 域 的 粒子 ,将 它们 以 随机 的 方式 分 开 。 这 种 减轻 样本 贫 化 的 方法 被 称 为 
正则 化 ,并 构成 了 采样 与 移动 过 程 中 的 移动 部 分 。 

除 文献 [4 -8] 讨 论 的 方法 之 外 ,还 有 一 些 重 采样 和 移动 的 方法 ,包括 马尔 
可 夫 链 蒙特 卡 洛 (MCMC ) 采 样 方法 和 以 MCMC 类 似 的 Gibbs 采样 方法 。MCMC 
采用 Metropolis - Hastings 接受 算法 取代 正则 化 步骤 ,这 些 方法 对 于 实时 递归 跟 
踪 应 用 来 说 计算 量 太 大 ,这 里 不 会 予以 考虑 。 然 而 ,在 某 些 情况 下 后 验 密度 有 大 
的 尾 概率 ,例如 a 稳定 分 布 情况 的 Levy 分 布 ,选择 适当 的 重要 性 密度 非常 困难 ， 
因此 标准 SIS 粒子 滤波 方法 可 能 会 失败 。 在 这 种 情况 下 ,粒子 滤波 器 中 引入 
MCMC 方法 ,提供 了 另 一 种 方法 用 来 构建 高 效 的 高 维 建议 分 布 。 对 于 MCMC E 
于 滤波 的 讨论 可 参阅 文献 [4] 及 其 包含 的 参考 文献 。 这 些 方法 还 可 以 应 用 到 静 
态 参 数 估计 和 平滑 方法 "当中 ,但 本 书 对 这 两 种 应 用 不 做 深入 讨论 。 
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16.2.1 逆 变 换 方法 


首先 查看 图 2 -7 中 的 一 维 累积 分 布 图 数 P(x) ,找到 解析 高 斯 分 布 从 
( -5,62) 的 值 。 如 图 所 示 ,与 解析 CDF 相关 的 累积 概率 在 区 间 (0,1] 内 均匀 分 
布 。 如 果 对 CDF 求 道 ,可 得 已 (xz) ,如 图 16 -1 所 示 。 如 果 在 均匀 分 布 (0,1] 
中 生成 样本 (ui ) ,并 利用 它 选择 一 个 沿 垂直 的 x 轴 (x;) 方 向 上 的 值 ,在 这 个 例子 
中 ,将 由 NM( -5,6”) 生 成 一 个 样本 。 注 意 ,在 这 个 例子 当中 ,只 在 -5 -13<x< 
-5+13 范围 之 外 选取 很 少 的 样本 。 


30 


20 


02 04 w 06 0.8 I 
图 16-1 高 斯 累积 分 布 的 逆 


通常 ,累积 分 布 及 其 道 有 如 下 属性 史 ””) : 

(1) 已 (D) 单 调 递 增 ; 

(2) P(P'(U)ZuH P(P 1(X) <x; 

(3) usP(X) 4 EHA P~ (U) <x; 

(4) 已 ”为 左 连续 。 

定理 16.1 "Eu -U(0,1) AAx~P'(U) =P(X) 

证 明 :P(X<x) =P(X<P™'(U)) -P(P^'(u)) =u=P(X) 

这 个 一 般 定理 仅 适用 于 解析 和 经 验 一 维 分 布 ,只 有 少数 特殊 情况 下 ,才能 应 
用 到 多 维 分 布 中 。 用 一 个 简单 的 例子 来 说 明 这 个 逆 分 布 采 样 方法 。 首 先 利 用 第 
2 章 给 出 的 方法 ,用 MATLAB 由 NM( -5,6 ) 生 成 50000 个 样本 。 然 后 ,由 一 个 经 
验 PDF 生成 一 个 13 条 的 直方 图 ,并 用 样本 的 总 数 进行 规范 化 。 条 与 条 之 间 的 
累加 和 构成 了 CDF。 图 16 -2 由 P(x) 与 x 及 x 与 P”(u) 之 间 的 关系 构成 。 注 
意 ,对 于 这 个 简单 的 例子 , 重 采样 的 过 程 只 能 生成 7 个 x 值 。 

有 很 多 数值 方法 可 实现 生成 样本 的 逆 运 算 。 这 里 只 考虑 其 中 3 个 :随机 采 
样 、 分 层 采样 和 等 距 采 样 。 然 而 ,研究 表明 ,“ 最 优 ” 结果 可 以 由 分 层 或 等 距 采 
FREU? 得 到 。 

对 于 随机 重 采 样 , 由 均匀 分 布 U(0,1) 生 成 样本 |u,i=1,…,n| ,然后 选取 
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16-2 高 斯 CDF 及 其 逆 的 对 比 


满足 x, -P (wu,) 的 值 |x;,i=1,…,n|。 列 表 16 -1 给 出 了 实现 这 一 功能 的 简 
单 的 MATLAB 源 代 码 。 

列表 16 -1 随机 采样 MATLAB 程序 片段 

% Random Resampling 

u =cumprod(rand(1,Ns). /[Ns: -1:1]); 

u=u(N: -1:1); 

wc = cumsum(w); 

label =nan(1,Ns); 

k=1; 


for i=1:Ns 
while(we(k) < u(i)) 
k=k+1; 
end 
label(i) =k; 


end 


$ Resample particles 

particles -particles(:,label); 

w=ones(1,Ns)./Ns; 

在 分 层 重 采样 当中 ,基于 调查 抽样 的 思想 ,将 区 间 (0,1] 预 分 割 为 ”个 均匀 
的 伴随 集 或 层 (0,1] = (0,1/n] U (1/n,2/n) U= UCn -17n,1], 24$, 4—2 
都 生成 了 单独 的 均匀 离散 数值 ,例如 第 i 层 , 有 ~U(i/n,i+1/n)。 因 此 可 设 
ix, =P (u) IIR 16 -2 给 出 了 实现 上 述 功能 的 MATLAB 代码 。 

列表 16 -2 分 层 采样 MATLAB 代码 片段 

% Strati fied Resampling 

u=([0:Ns -1] +rand(1,Ns)/Ns; 

wc = cumsum( w) ; 
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label =nan(1,Ns); 
k=l: 


for i=1:Ns 
while(wc(k) < u(i)) 
k=k+1; 
end 
label(i) =k; 


end 


% Resample particles 

particles =particles(:,label); 

w =ones(1,Ns). As; 

最 后 ,在 等 距 末 样 中 ,均匀 区 域 被 再 次 细 分 为 n 个 均匀 的 伴随 层 , 与 分 层 采 
样 相同 ,但 对 于 所 有 层 只 有 一 个 单独 的 随机 数 绘制 。 列 表 16 -3 给 出 实现 上 述 
算法 的 MATLAB 代码 。 

列表 16 -3 等 距 采 样 MATLAB 代码 片段 

% Systematic Resampling 

u=([0;Ns-1]+rand(1,Ns))/Ns; 

wc = cumsum( w); 

label -nan(1,Ns); 

k=1; 


for i=1:Ns 
while(we(k) < u(i)) 
k=k+1; 
end 
label(i) =k; 


end 


% Resample particles 

particles =particles(:,label); 

w=ones(1,Ns). /Ns; 

在 本 书 中 所 有 需要 进行 粒子 滤波 器 重 采 样 的 案例 分 析 中 , 均 采用 等 距 重 
采样 。 


16.2.2 带 重 采样 的 序 贯 重要 性 采样 粒子 滤波 器 


可 以 使 用 逆 变 换 方法 在 SIS 粒子 滤波 器 中 进行 重 采 样 。 对 于 式 (16 -11 ) 中 
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给 出 的 离散 分 布 p(x,lz.,.), Ë JE BARS B p(x, lz.) AS PKM Ur. H 
式 (2-80) 中 给 出 的 累积 分 布 的 定义 ,得 


P(x” | Zin) = Pr | y, | E. = x] Zial = EA Zn) dy, 
= [. 2, wy 8(y, - y. )dy, = iw) (16 - 15) 
这 样 ,离散 分 布 p(z,1zi,,) 的 每 个 粒子 对 CDF 贡献 其 权 值 ,主要 由 于 CDF 
是 由 PDF 通过 积分 得 的 ,这 是 一 个 非常 有 趣 的 结论 。 第 2 章 提 出 了 一 种 多 维 
CDF ,这 种 CDF 的 分 量 包 含 的 维 数 与 PDF 相同 。 对 于 离散 p(x,1z.,) ,尽管 状态 
矢量 x, 和 p(x,1zi,, ) 是 多 维 的 ,也 可 以 仅 由 权 值 构造 一 维 CDF。 因 此 可 以 利用 
道 变 换 方法 对 离散 多 维 p(x,1z,, ) 进行 重 采样 。 
这 些 粒 子 滤 波 重 采 样 步骤 通过 修改 加 权 近 似 密 度 ,消去 重要 性 权 值 低 的 粒 
子 , 生 成 未 加 权 密 度 。 低 权 值 粒子 的 去 除 是 通过 复制 一 个 高 重要 性 权 值 粒子 来 
取代 低 权 值 粒子 ,以 保持 粒子 的 数量 不 变 。 因 此 有 


N, 
p(x,l z...) = Mwió(x, — xt”) (16-16) 
š i=l 


RRA 


N N, 


pln) x8, — x; ) = Y mar, =x) (16-17) 


这 里 ky f x; | 的 重 采样 集合 中 粒子 x,” 副 本 的 数目 。 参 见 文献 [10] 对 这 

些 以 及 其 他 SIS 粒子 滤波 重 采样 方法 的 完整 评价 。 在 SIS 粒子 滤波 器 的 许多 应 

用 中 ,由 扩散 迁移 引起 的 离散 粒子 密度 远离 真实 密度 的 粒子 退化 情况 并 不 严重 ， 

因此 不 需要 在 每 次 迭代 中 都 进行 重 采样 。 对 于 特殊 应 用 来 说 ,合适 的 退化 衡量 

指标 是 有 效 采 样 大 小 Na, 可 以 由 文献 [11 |] 进行 估算 , 即 
Na = N, 

> (wf? )? 

如 果 所 有 的 权 值 都 相等 , 即 w = 1/N, IBAA Ne =N,。 然 而 ,如 果 粒 子 退 化 非 

常 严重 ,例如 对 于 粒子 i, 有 ws ”=1; 而 对 于 所 有 其 它 粒子 , 权 值 均 为 零 , 此 时 

Nu =1。 因 此 N. <N, 表示 退化 严重 ,而 Nun ~ N, 表示 退化 很 微小 。 

表 16 -2 给 出 了 重 采样 SIS 粒子 滤波 器 过 程 。 
表 16 -2 带 重 采样 的 序 贯 重 要 性 采样 粒子 滤波 器 


(16-18) 


|. 滤波 器 初始 化 
(1) 初始 化 状态 矢量 样本 ; xj) —q(xo) ¿i=l,--,N, 


(2) 初始 化 权 值 





( 续 ) 
2. 序 贯 重要 性 采样 
(1) 选取 新 样本 ; (i). o(x, | xt z), Vi 
(2) 生成 非 规范 化 的 重要 性 权 值 ; a = wi, pz, | xi? p(k? 1x25) 


g (xD? 12109,,.2.) 


(3) 规范 化 权 值 


4. M N. < N, 
(1) 重 采 样 ; 利用 MATLAB 重 采 样 例 程 进行 重 采 样 
(2) 重 置 权 值 ws = 1/N, 
end if 
5. 如 需要 ,计算 和 矩 
(1) 一 阶 矩 :状态 矢量 均值 ; š - Year 


(2) 二 阶 矩 : 协 方差 


N, 
Pr = Yf aP -DGP -3,)" 
mj 


6. 时 间 递 增 , 返 回 步 又 2. 1 xj xl sw — wt? 





16.2.3 正则 化 


重 采样 之 后 ,在 结果 粒子 的 样本 集合 中 ,对 于 任意 给 定 的 权 值 ,都 包含 很 多 
重复 的 粒子 。 在 文献 中 这 种 问题 被 称 为 样本 贫 化 。 为 修正 由 重 采样 所 导致 的 样 
本 贫 化 ,在 每 次 重 采 样 过 程 之 后 ,可 以 利用 粒子 密度 的 核 密度 估计 来 对 粒子 进行 
第 二 次 重 采样 。 在 这 个 过 程 中 ,每 个 新 粒子 (蒙特 卡 洛 样本 ) 都 是 在 原始 重 采 样 
粒子 中 均匀 分 布 选 取 , 然 后 基于 局 部 核 函 数 ,对 采样 点 进行 微小 的 移动 ( 拌 动 ) 。 
这 个 正则 化 过 程 往往 集中 在 粒子 概率 最 高 的 区 域 ,用 随机 的 方式 把 它们 分 开 。 
图 16 -3 给 出 这 一 过 程 的 完整 结果 的 示例 。 

图 16 -3 中 给 出 了 后 验 密 度 和 重要 性 密度 。 样 本 取 自 重要 性 密度 ,其 位 置 
Mer x 轴 方 向 ,用 编号 来 描述 。 根 据 比 率 o,” = p(x)” 274 (x ) ,为 每 个 粒子 分 配 
一 个 非 规范 化 权 值 ,图 16 -3 最 上 面 一 行 编号 的 半径 表示 权 值 的 相对 大 小 。 

这 些 数字 占 位 符 用 来 表示 继承 关系 , 即 每 一 步 结 束 时 各 个 粒子 的 最 终 位 置 。 
第 二 行 表示 粒子 重 采 样 后 ,所 有 的 粒子 都 有 相等 的 权 值 , 低 权 值 粒子 变 为 现 有 高 
权 值 样本 的 副本 。 第 三 行 表示 样本 正则 化 以 后 , 即 在 重 采样 后 ,移动 粒子 ,使 得 
没有 两 个 粒子 具有 完全 相同 的 x 值 。 

表 16 -3 给 出 了 多 维 状 态 矢量 的 重 采样 并 正则 化 (移动 )SIS 粒子 滤波 器 过 
程 。 注 意 ,经 验 协 方差 矩阵 D, 的 计算 必须 在 重 采样 步骤 之 前 进行 ,因此 是 x)” 
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图 16-3 重 采样 原理 
上 面 : 从 4( 虚线) 中 选取 样本 ,用 半径 与 规范 化 权重 成 比例 的 编号 来 表示 相关 的 规范 化 重要 
性 权 值 (p 对 应 的 目标 密度 用 实 线 来 标 绘 ) ; 
中 间 :在 重 采样 之 后 ,所 有 的 粒子 具有 相同 的 重要 性 权 值 ,其 中 一 些 已 经 复制 过 ; 
下 面 : 样 本 已 进行 了 规范 化 ,利用 核 密度 估计 器 进行 重 采 样 和 移动 。 


和 wl 的 函数 。 因 为 正则 化 步 又 由 后 验 密度 进行 重 采 样 ,所 有 粒子 在 矩 积分 估 
计 总 和 中 有 着 相等 的 重要 性 ,所 以 将 所 有 的 权 值 重 置 为 w 7” = 1XN。 在 每 一 次 迭 
代 之 后 ,可 在 时 间 递 增 之 前 进行 矩 的 计算 。 

可 以 看 出 , 重 采样 .正则 化 的 SIS 粒子 滤波 仍然 无 法 应 用 于 跟踪 问题 。 在 确 
定 合适 的 重要 性 分 布 9(x,1x,_1,z,) 以 及 似 然 函 数 plz, lx) 的 解析 形式 上 ,还 存 
在 很 多 问题 。 

表 16 -3 序 贯 重要 性 采样 粒子 滤波 器 正则 化 
|. 滤波 器 初始 化 
(1) 初始 化 状态 矢量 样本 ; xj? ~ q(xo) i 


(2) 初始 化 权 值 
2. 序 贯 重要 性 采样 
(1) 选取 新 样本 ; x) ~ q(x 1 x!) z.) , Vi 


wD, PCa l xn )P(zn | xncl) 
E q(x | x£9,,2,) 


(2) 生成 非 规范 化 的 重要 性 权 值 ; m = 


N, 
(3) 规范 化 权 值 wf) = ES 





( 续 ) 


4. If Ng < N, 


(1) 计算 经 验 平均 值 


Ns 
x, = oe -p,) * (x -p,)" 


(2) 计算 经 验 协 方差 
x, = D,D" 

(3) 粒子 重 采 样 利用 MATLAB 重 采样 例 程 进行 重 采样 
(4) 粒子 正则 化 利用 表 15 - 3 中 的 步骤 ,实现 粒子 正则 化 
(5) 时 间 递 增 ,返回 步骤 2. 1 xt) —xQ swf? = 1⁄N 
Else 

时 间 递 增 ,返回 步 又 2. 1 XQ) 一 

end if 





16.3 自 举 粒 子 滤波 器 


自 举 粒子 滤波 器 (BPF ) 在 文献 [12] 中 首次 提出 ,因为 最 容易 实现 ,也 是 使 
用 最 为 广泛 的 重 采样 SIS 粒子 滤波 器 。 在 自 举 粒 子 滤波 器 中 ,选择 转移 密度 作 
为 重要 性 密度 , 即 


q(x | x,4,2,) = p(x, | x,.,) (16 - 19) 
由 于 选取 了 重要 性 密度 ,权重 更 新 方程 (16 - 12) 变 成 
w^ = wP p(z, | x?) (16 -20) 


自 举 粒子 滤波 器 的 特征 是 :权重 的 增 量 不 依赖 于 过 去 的 粒子 轨迹 ,只 取决 于 
观测 值 p(z,1x, ) 的 条 件 似 然 函数 。 自 举 粒 子 滤波 器 的 采样 非常 简单 ,用 状态 转 
移 方 程 (5 - 1) 转移 上 一 时 间 步 中 的 粒子 ,生成 新 的 粒子 。 然 后 是 重 采样 和 移动 
步骤 , 表 16 -4 给 出 了 完整 的 BPF 过 程 。 同 样 ,分 布 的 一 阶 和 二 阶 矩 可 以 在 每 
个 时 间 步 结束 时 进行 计算 。 

需要 强调 一 点 , 表 16 -4 中 ,在 状态 协 方差 计算 时 缺少 噪声 协 方差 矩 阵 Q - 
这 里 并 不 需要 计算 O, AARE v ~ pCv,) 的 抽样 已 经 包含 在 状态 矢量 的 转 
移 方程 中 。 因 此 ,这 里 也 需要 动态 噪声 密度 的 信息 。 因 为 最 初 的 假设 是 针对 后 
验 密度 未 知 的 情况 ,那么 假定 动态 噪声 的 解析 形式 违背 了 最 初 的 假设 。 绕 过 这 
一 问题 的 常用 方法 是 忽略 噪声 项 ,在 粒子 转移 过 程 中 不 添加 噪声 。 

还 有 就 是 需要 找到 似 然 函 数 愉 zlz,) 的 解析 表达 式 。 如 前 所 述 ,p(z,lz，) 
通常 被 认为 是 高 斯 明 数 。 但 对 于 高 度 非 线 性 观测 方程 ,观测 噪声 可 以 是 非 高 斯 
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的 ,例如 在 DIFAR 实例 当中 。 

用 自 举 粒子 滤波 器 解决 实际 问题 时 , 主要 问题 是 当 观 测 噪声 很 小 的 时 候 ， 
p(z, 1x, ) 是 一 个 非常 窗 的 函数 。 这 时 ,转移 粒子 的 p Cz, 10; ) 值 非常 小 ,导致 相应 
的 更 新 权 值 也 非常 小 。 在 这 种 情况 下 ,需要 在 每 一 次 更 新 时 进行 重 采 样 和 移动 步 
又 。 另 一 方面 ,如 果 假 设 pz, 1x, ) 为 高 斯 分 布 (实际 上 并 不 是 ) ,在 观测 值 当 中 会 
有 无 信息 的 异常 值 , 例 如 所 有 粒子 p(z,1x,”) 值 都 为 零 将 导致 滤波 器 不 稳定 。 

X16-4 带 重 采样 和 正则 化 的 自 举 SIS 粒子 滤波 器 
1 滤波 器 初始 化 
(1) 初始 化 状态 矢量 样本 ; x ~ alx) i=1,--,N, 
(2) 初始 化 权 什 wi? = 六 
2、 序 贯 重要 性 采样 
(1) 选取 新 样本 ; 


x,” ~ p(v,) 


xi = f(x) * vi? Vi 


(2) 生成 非 规范 化 的 重要 性 权 值 ; SU = wid plz, | xt) 


N, 
(3) 规范 化 权 值 wi) = m7 2 9 


4. If Nu < N, 


(1) 计算 经 验 平均 值 ; M, = 


N, 
l i i 
F, =J 28 -a,) GP -mo 


(2) 计算 经 验 协 方差 ; 
>, = D,D] 

(3) 粒子 重 采样 ; 利用 MATLAB 重 采 样 例 程 进行 重 采样 
(4) 重 采 样 粒子 正则 化 ; 利用 表 15 - 3 中 的 步骤 ,实现 粒子 正则 化 
(5) 时 间 递增 ,返回 步骤 2.1 x ex sol) = 1⁄N 
Else 

时 间 递 增 ,返回 步骤 2.1 

end 让 





16. 3.1 BPF 在 DIFAR 浮标 跟踪 中 的 应 用 


第 4 章 式 (4-34) 给 出 了 DIFAR 浮标 的 似 然 函 数 pO, nl On) ,用 来 表示 1 

时 刻 第 m 个 DIFAR 元 素 的 方位 测量 值 9,, ,以 整个 空间 的 可 能 目标 方位 | - =< 

On ST) 为 条 件 , 作 为 浮标 输入 的 信号 信 品 比 。 高 信 噪 比 信号 的 规范 化 似 然 函 数 
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为 高 斯 的 ,而 低 信 噪 比 的 接近 于 均匀 分 布 , 这 并 不 出 人 意料 。 因 为 在 信号 缺失 
CIC (ER E ) 的 情况 下 ,浮标 的 输入 是 在 | — m SO, m m1 区间 呈 均 匀 分 布 的 噪 
声 。 假 设 噪声 独立 于 浮标 ,可 以 将 浮标 场 似 然 函 数 写成 单个 浮标 函数 的 乘积 ,并 
将 其 应 用 到 表 16 -4 自 举 粒 子 滤波 器 过 程 中 的 第 2 步 。 

为 初始 化 自 举 粒子 滤波 器 ,使 用 与 7.4.3 中 相同 的 步 又。 在 计算 初始 目标 
状态 xo 估计 之 后 ,生成 一 组 初始 化 粒子 1xzo ,i =1,…,N,| ,有 


x” = E E (16 -21) 


其 中 m ~ 人 (0,C) 。 顺 序 执行 表 16 -4 中 其 余 的 步骤 ,得 如 图 16 -4 所 示 的 估 
计 目 标 轨迹 与 真实 轨迹 的 对 比 。 这 里 设 信 噪 比 为 20dB ,初始 距离 为 4n mile, jË 
度 为 30kn( 实 际 的 初始 距离 和 速度 大 约 分 别 为 3. Sn mile 和 30kn) 。 对 于 这 个 例 
子 , 设 粒子 总 数 为 1000。 


3 


Y Abb /n mile 
Lh o = wv 


| 
N 


-3 

一 4 

-2 -1.5 -1 -05 0 0.5 l ].3 2 
x 坐标 /n mile 


图 16 -4 利用 自 举 粒子 滤波 器 实现 DIFAR 浮标 场 的 目标 跟踪 


图 16 -5 表明 6 种 不 同 输入 信和 号 信 噪 比 ( 从 20dB 降 至 0dB, 步 长 为 5dB ) 的 
情况 下 , 自 举 粒 子 滤波 器 的 跟踪 输出 结果 与 从 前 的 DIFAR 跟踪 估计 中 的 初始 化 
相同 。 在 这 个 例子 中 ,BPF 使 用 了 3000 个 粒子 。 与 图 7 -3 给 出 的 类 似 的 EKF 
标 绘 图 相 比 ,可 发 现在 所 有 信 噪 比 下 ,EKF 的 跟踪 结果 都 比 BPF 更 精确 。 而 且 ， 
在 每 种 信 品 比 下 ,100 次 BPF 蒙特 卡 洛 运行 都 出 现 了 发 散 轨迹 。 在 后 面 将 讨论 
产生 BPF 性 能 缺陷 的 一 些 原 因 。 

与 高 斯 卡尔 曼 滤 波 器 不 同 ,BPF 的 性 能 对 距离 的 初 值 非常 敏感 。 如 果 初 始 
距离 大 于 5n mile, BPF 航 迹 将 会 发 散 。 增 加 粒子 的 数目 会 在 一 定 程度 上 缓解 这 
一 问题 ,但 可 以 发 现 ,由 于 其 对 初 值 的 敏感 ,BPF 实际 很 难 使 用 ,BPF 似乎 对 于 初 
始 速度 的 变化 不 太 敏 感 。 本 书 试图 通过 选择 无 信息 的 均匀 分 布 距离 和 速度 来 初 
始 化 粒子 ,距离 范围 2n mile - 5. 9n mile , 增 量 0. 1n mile ,速度 范围 24kn ~ 30kn, 
增 量 为 0. 5kn ,但 多 数 跟踪 估计 都 是 高 度 发 散 的 。 虽 然 在 文献 中 很 少 提 及 ,这 种 
初 值 敏感 性 使 BPF 难以 应 用 于 真实 场景 中 ,除非 有 另 一 种 跟踪 方法 来 生成 “好 
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的 ” 初 值 。 此 外 ,必须 注意 的 是 ,对 BPF 算法 ,其 重要 性 密度 不 考虑 当前 观测 值 ， 
这 使 BPF 成 为 次 优 滤波 器 选择 。 虽 然 BPF 算法 很 容易 实现 ,但 相 比 一 些 下 面 将 
提出 的 粒子 滤波 方法 来 说 , 它 有 严重 的 缺陷 。 
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图 16 -5 六 种 信 噪 比 下 BPF 跟踪 器 输出 结果 比较 


16.4 最 优 序 贯 重要 性 采样 粒子 滤波 器 


为 克服 BPF 的 上 述 问题 ,需要 合理 地 选择 重要 性 密度 。 因 此 ,在 设计 粒子 
滤波 器 时 ,重要 性 密度 的 选择 是 最 关键 的 问题 。 重 要 性 密度 的 最 优选 择 是 使 权 
值 的 方差 最 小 。 权 值 的 均值 和 方差 可 表示 为 


{wr} = fw at | x? z )dx, (16 -22) 


MICE 
* (i) = *(i) 12 * (i) 2 
Var cagots Iw, | = E (x61 hd) sza) ILE [Scape as? Iw, 上 


(16 - 23) 
如 果 首 先 检查 均值 方程 (16 -22) ,将 其 代入 式 (16 -12) ,得 w” 的 递归 方程 为 


hw ,| = wi [Pe | x,)p (x, | x dr, 


e . 
q xf 0 x 2. z,) 


= p( z. | xB) (16 -24) 


对 方差 方程 (16 - 23) 的 第 一 项 采用 相同 的 步骤 , 并 利用 式 (16 - 24) 计算 
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式 (16 -23) 的 第 二 项 ,得 


» 2 [p(z, | x,)p (x, | pa )] 
Vaca wet = ot Te 
dx, - [p(z, | x2) ] (16 -25) 
根据 贝 叶 斯 定律 ,如果 满足 下 式 , 式 (16 -25) 可 化 简 为 零 '"” 
本 (16 - 26) 
(z,] x, x", )p (x, 1) x9) 
EET KT S Fr É (16 -27) 
这 样 ,重要 性 权 值 更 新 方程 (16 - 12) 的 表达 式 变 为 
(i) Ci) 
w” s sp PERDE P ie (16 - 28) 
= wi p(z, | x0) (16 -29) 


将 BPF 权 值 方程 (16 - 20) 5j OPF BUB; Pë (16 -29) 进 行 比较 ,可 以 看 到 ,OPF 
重要 性 权 值 OO 不 再 依赖 于 x。 因此 ,1, 时 刻 的 重要 性 权 值 可 以 在 粒子 传播 
至 时 刻 之 前 计算 得 出 。 
使 用 最 优 重要 性 密度 有 两 个 缺点 9 。 首 先 ,必须 可 以 由 重要 性 密度 p(x 
x0, ,z,) 采 样 ;其 次 ,必须 能 将 p (2, |.) 计算 到 一 个 规范 化 常数 ,其 中 P(z| 
x ) 由 积分 求 得 , 即 
p(z, l x09) = [ p(z, 1 z.)p(x,1 x dx, (16 -30) 
通常 情况 下 ,这 两 个 任务 都 是 非常 困难 的 ,然而 ,也 有 一 些 有 趣 的 特例 可 以 应 
用 最 优 SIS 粒子 滤波 器 。 如 文献 [3,15 ] 中 指出 : 当 x, 是 有 限 集合 的 成 员 时 ， 
第 一 种 特殊 情况 出 现 , 式 (16 -30) 中 的 积分 将 变 成 求 和 ,p(x, 1 uz, ) 采 样 
变 成 可 能 性 。 这 种 特殊 情况 的 例子 是 用 线性 跳 路 马尔 可 夫 过 程 处 理 机 动 目标 
跟踪 。 下 面 将 处 理 的 是 第 二 种 情况 ,p(x, 1 x.,,z,) 是 一 类 高 斯 分 布 的 
PRY) 
16.4.1 高 斯 最 优 序 贯 重要 性 采样 粒子 滤波 器 
假设 系统 动态 转移 模型 是 非 线性 的 ,具有 加 性 高 斯 噪声 ,而 观测 方程 是 线性 
的 , 带 有 加 性 高 斯 噪声 。 这 样 一 个 系统 可 以 描述 为 
x, = f(x. ,) + p... (16 -31) 
z, = H,,, + w, (16 -32) 
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v, ~ N(0,Q,) (16 -33) 


w, ~ N(0,R,) (16 -34) 
985 BE p(x, 1x, ., ) I p(z, 1x, ) TEMA 
P(x, | x,4) = N(G,sf(x,4),0..) (16 -35) 
p(z,| x,) = N(z, ;H,x,,R,) (16 -36) 
由 贝 叶 斯 法 则 ,得 
P(X, | x,4,2,) = piza rm (16 -37) 
由 上 得 


p(z, | x,)p(x, | X44) = p(x, | X,_, ,z,)p(z, | x,-1) (16 -38 ) 


由 于 等 式 左边 的 乘积 是 高 斯 分 布 的 乘积 ,那么 右边 的 乘积 一 定 是 高 斯 分 布 的 乘 
积 。 因 此 , 令 


p(x,| x,4,,2,) = N (x,;a,,XZ,) (16 -39) 
p(z,| x,.,) = N (z,:b,,S,) (16 -40) 
这 样式 (16 -38 ) 可 以 写成 高 斯 密度 项 , 即 
N (z,;H.x, Ra) N (x,sf(x,4),0Q,a4) = N(z.;b,,S,) N'(x,;a, E.) 


(16-41) 
解析 高 斯 密度 的 指数 项 相等 ,经 过 代数 运算 ,得 
b, = H, f(x,_,) (16 -42) 
S, = H,0,_,H' +R, (16-43) 
a, = f(x,1) + X,H;R, (z, - b,) (16 -44) 
X. = Q, -0Q.H.s. 8,0... (16 -45) 


注意 ,如 果 Q, RA H, 与 时 间 无 关 , 那 么 S, MY, 也 是 时 间 无 关 的 ,可 以 离线 
计算 。 

因为 这 是 一 个 粒子 滤波 器 ,可 以 将 粒子 1x i= 1, NY RA SR (16 -42) 
和 式 (16 -44) ,所 以 必须 令 b,—b;" H.a,—a;^. 3€16 -5 给 出 了 OPF 的 完整 
步骤 ,符号 xi ,表示 状态 预测 和 中 间 的 计算 步骤 中 的 预测 粒子 。 在 中 间 的 计 
算 步 又, 用 z, 表示 1 时刻 的 观测 值 。 因 为 SIS 粒子 滤波 器 需要 进行 重 采 样 , 假 
设 在 每 个 时 间 步 都 需要 进行 重 采样 ,也 可 以 改 成 当 N. <N, 且 需 要 时 进行 重 采 
样 。 注 意 ,t, 时 刻 的 重要 性 权 值 是 在 重要 性 采样 步骤 中 粒子 向 前 传播 之 前 ,在 权 
值 更 新 步骤 进行 计算 。 而 且 , 旧 的 粒子 也 可 以 在 粒子 在 时 间 上 传播 之 前 进行 重 
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采样 , 权 值 设 为 1/N,。 
表 16 -5 带 重 采样 和 正则 化 的 最 优 SIS 粒子 滤波 器 
1. 滤波 器 初始 化 
(1) 初始 化 状态 矢量 样本 ; 
(2) 初始 化 权 值 
xi a = (xb) 
b) = xf), 
S, = H,Q,_, HT +R, 
3. 权 值 更 新 


(1) 似 然 函 数 p (z, | xÜ ) = N (z, bf” ,S,) 


(2) 权 值 更 新 ux?) ES wl p(z, | xi?) 


N, 
(3) 规范 化 权 值 wi = wo /Tn 


4. 粒子 重 采样 利用 MATLAB 重 采 样 例 程 进行 重 采样 


5. 移动 重 采样 粒子 采用 表 15 - 3 中 的 步骤 对 粒子 进行 正则 化 
重 置 权 值 wi? = 1/N 


6. 重要 性 采样 
(1) 中 间 计 算 ; at? = X d + 3, ALR," (z, - bf") 


(2) 重要 性 采样 Z, = Qi- sg Q. BIS H,.Q,, 


rH š p(x, | Xn-l Za) = N(x, an X.) 


x, € Y, wi) xi?) 
= (D ot) _ S$. 2T 
rm ^w Y vu x, X, xXx, 


[s nunan. — | | 
16.4.2 ”局 部 线性 化 的 高 斯 最 优 序 贯 重要 性 采样 粒子 滤波 器 


当 动态 方程 和 观测 方程 均 为 非 线 性 时 ,如 果 用 某 种 方式 ”将 非 线 性 观测 方 
程 线性 化 ,仍然 可 以 用 OPF。 考 虑 观测 方程 





z, = h(x,) + w, (16 -46) 
将 h(x,) 展 开 为 关于 f(x,_, ) 的 多 维 泰勒 级 数 ,利用 式 (2 762) 18 
h(x,) = h(f(x,,)) +H,(x, -f(x,,)) (16 -47) 
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雅 可 比 矩 阵 H, 定义 为 


B, à (Woes) T anuo (16 -48) 
这 样式 (16 — 46) E JV, 
z,~H,,, +, (16 -49) 
其 中 
Z, à z, + Ñ, f(x,.) - h(f(x,1)) (16 -50) 


ORE ,把 观测 方程 变 成 在 x, 为 线性 ,代价 是 在 观测 方程 中 增加 额外 的 项 ， 
即 ,z, 一 Zz,。 可 以 利用 表 16 -5 PRI, HS H, >À, H z2, ,额外 增加 雅 


8I kE £ A, 的 计算 和 额外 的 z, 项 ,在 式 (16 -49) 和 式 (16 -50) 中 进行 x, 一 
x 和 x,_1 一 x 中 的 替换 。 

只 要 所 有 密度 都 是 高 斯 的 ,可 以 将 OPF 应 用 于 非 线 性 动态 方程 和 观测 方程 
的 任意 跟踪 问题 。 对 于 多 数 有 上 述 特 征 的 跟踪 问题 ,使 用 OPF 并 未 获得 比 本 书 
第 卫 部 分 给 出 的 所 有 方法 更 好 的 结果 。 然 而 ,在 一 些 实例 中 可 能 需要 对 跟踪 解 
施加 约束 ,例如 将 跟踪 估计 限制 到 一 个 预先 确定 的 道路 网 格 中 ,或 限制 目标 状态 
的 可 操作 性 或 速度 。OPF 最 适合 解决 这 种 类 型 的 跟踪 问题 。 因 为 DIFAR 跟踪 
实例 中 对 目标 状态 没有 任何 限制 ,本 书 不 会 将 局 部 线性 化 高 斯 最 优 SIS 粒子 滤 
波 算法 应 用 到 这 个 案例 中 。 


16.5 序 贯 重要 性 采样 辅助 粒子 滤波 器 


选择 一 个 合理 的 SIS 建议 分 布 g(x, 1x, a Zn) ,会 保证 将 当前 的 观测 值 z, 并 
人 建议 步骤 ,这 样 粒 子 不 会 盲目 地 移入 没有 给 出 观测 值 的 状态 空间 区 域 。 显 然 ， 


BPF 无 法 实现 这 一 目标 ,因为 在 g(x, 1x, 1,z,) =p(x, 1x,_,) 中 没有 考虑 当前 观 


测 值 。 如 前 所 述 ,OPF 选择 g(x, lx, iz) =p (ty le, az) ,但 除了 在 特殊 情况 
下 ,OPF 很 难 实现 。SIS 辅助 粒子 滤波 器 (APF) 是 一 种 以 次 最 优 的 方式 将 当前 
观测 值 加 入 到 建议 分 布 的 粒子 滤波 器 。 

在 包括 BPF 在 内 的 标准 重 采样 SIS 粒子 滤波 中 ,在 从 重要 性 密度 中 抽取 粒 
子 和 通过 当前 观测 值 更 新 粒子 权 值 后 ,对 粒子 进行 重 采样 。 另 一 方面 ,在 OPF 
中 , 重 采样 发 生 在 由 重要 性 密度 中 抽取 粒子 之 前 , 通过 对 比 表 16 - 5 与 
表 16 -2、 表 16 -4 可 以 证 实 这 一 点 。 在 文献 [16] 和 第 13 章 的 文献 [17 ] 中 Pitt 
和 Shephard 提出 将 APF 作为 标准 SIS 的 变 体 ,该 算法 的 重 采样 步骤 利用 ,时刻 
的 观测 值 , 在 1,_, 时 刻 执行 。 通 过 这 种 方式 ,APF 试图 模拟 最 优 粒子 滤波 器 所 使 
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用 的 方法 。 在 文献 [7,8 ] 中 可 以 找到 APF 步骤 的 清晰 描述 ,本 节 中 的 部 分 内 容 
也 是 直接 引 自 文献 [7] 。 
APF 的 主要 思想 是 增加 权 值 大 的 ( “好 的 ”) 粒子 zx ,在 某 种 意义 上 ,这 些 
粒子 的 预测 依然 p(z. |x ) 很 大 。 根 据 贝 叶 斯 法 则 (3 - 23) ,后 验 密 度 可 写 为 
p(x, | z,,) cc p(z, | x,)p(x,]| ZI) (16-51) 
利用 Chapman - Kolmogorov 方程 (3 -24) ,将 上 式 变 为 


p(x,| 2.) © PCZ, 1 x,) [p(x, | x, px, l z,, dx, (16-52) 


如 果 有 


p(x,, l z...) = Y «i, -x\"\) (16 -53) 
那么 则 有 
p(x,1z,,) « | X wp, | x) [p(z, | z.) (16 -54) 


PRR w, plax, lxi) 当 作 联 合 概率 ,用 于 计算 滤波 后 的 密度 总 和 。 

一 种 从 经 验 预测 密度 中 采样 的 方法 是 将 于 ,ws p(x, 1x' , ) 当成 一 个 “ 先 
R RARE PO, Zn) ,将 其 与 似 然 度 p(z, 1x, ) 相 结合 ,产生 后 验 密度 。 样 
本 x 可 以 由 P(x,1zi,,_ 1 ) 中 抽取 ,通过 选取 x 和 概率 w Hop Cae, Lael?) FP 
行 选取 。 

为 理解 APF 方法 ,在 采样 g(x, Ix, 1,z, ) 之 前 ,首先 检查 SIS 更 新 方程 (16 - 12), 
将 其 重 写 为 

NN p(z, | x,)p( x, Er 
~ ge, a ut) 
引入 一 个 辅助 变量 £(£ © ll, N. | ,其 索引 是 “ 先 验 "混合 密度 的 一 个 分 量 。 
将 联合 条 件 索 引 后 验 密度 定义 为 p(x,,é =ilzi,, ) WA 
p(x,,£ =il z,,) & p(z, 1 x,)p(x,,£ = il z,, ) 
= p(z, | x,)p(x, | E = ioz,, ,)pCl z,, ,) 


(16 -55) 


(16 -56) 

将 上 式 与 式 (16 -54) 进 行 对 比 ,得 
p(x,| £ = iz, ,) = p(x, | xi) (16 -57) 
p(ilz 4) = we (16 -58) 


Baz xi ,zw | ,得 x, 后 验 概率 为 
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B. à p(x,,£ = il z,,) = p(z,| x,)p(x, | x. wi, (16-59) 

Xl iE ms £, n] A HIER SEE (16 -59) 抽 取样 本 。 假 设 由 边缘 密度 p (x, | 

zi) 中 抽取 一 组 粒子 1xz。 15,891 £ AS pC£ 12,4) = ws 成 正比 的 概率 模 
拟 。 这 样 , 式 (16 -54) 可 由 下 式 估计 求 得 , 即 


N 


p(x, 1 z...) « Y wz, | x, b EAE (16 -60) 


式 中 :&"" 为 粒子 x,” 在 时 间 步 cu 89285] (Br e" A d£ il). 
用 来 抽取 样本 jx ,i =1,… N | 的 建议 分 布 ,以 一 种 分 解 的 方式 选取 , 即 


q(x,,£lz,,) « q(£l z,,)q(x, | £,z,,,) (16 -61 ) 
q(€ 12,.) = wap(z, | gen”) (16 -62) 
q(x, | £P z ) = p(x, | zÜ (16 -63) 


XE po) JEG p(x, Leh, ) 相关 的 数值 (例如 均值 模式 或 样本 值 ) ,由 此 抽取 第 i 
个 粒子 。 
由 下 式 进 一 步 通 近 真 正 的 后 验 概率 , 即 


N, 
p(x, 1 z,,) © Y wip(z,| uit? )p(x,| x55?) (16 -64) 
ial 


这 样式 (16 -55 ) 的 非 规范 化 权 值 变 成 
c Q aplenty PG ly p(y? I rE) _ pz, E xz?) 

n n-i q (xi? E” | Zin) ~ p(z, | p 7?) 
表 16 -6 给 出 了 辅助 粒子 滤波 器 的 过 程 流 程 表 。 为 总 结 SIS APF 的 处 理 流程 ， 
第 2 步 相当 于 使 用 自 举 粒子 滤波 器 计算 一 组 新 的 权 值 , 然 后 使 用 这 些 新 的 权 值 
对 粒子 集 |x, ,i=1,… N 进行 重 采样 和 正则 化 。 继 而 在 第 5 步 重 采样 后 的 
粒子 进行 时 间 向 前 传播 ,利用 式 (16 - 65 ) 计 算 一 组 规范 化 的 新 权 值 。 

表 16 -6 辅助 粒子 滤波 器 过 程 流程 表 


(16 —65) 


1. 滤波 器 初始 化 
(1) 初始 化 状态 矢量 样本 ; xj) ~ q(x0) i = 1,.…,N, 


(2) 初始 化 权 什 wj) = — 


uA? mean or draw from p(x, | x(^, ) , Vi 


Bi? = wt p(z l ui? ), Wi 
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( 续 ) 


4. 利用 BO BORE xU 利用 MATLAB 重 采样 例 程 进行 重 采样 x 用 Bl? HR 
代 w^ 
移动 重 采样 粒子 用 表 15 - 3 中 的 步骤 对 粒子 进行 正则 化 


X =: p( x, | 有 " ad. 
p(z, | xí) 


p(z, | ast") 


N, 
PE. (U) 
a = Ww, > 
iz 





APF 基本 上 分 为 两 个 阶段 :在 第 一 阶段 ,仿真 具有 大 的 预测 似 然 性 的 粒子 ; 
在 第 二 阶段 ,重新 计算 粒子 权 值 ,抽取 增 大 状态 ,这 相当 于 做 一 个 建议 , 选 高 条 件 
似 然 性 的 粒子 作为 先 验 , 以 此 避免 无 效 的 抽样 。 因 此 ,APF 利用 从 似 然 模型 中 得 
的 事前 信息 ,以 避免 无 效 抽样 ,因为 低 似 然 性 的 粒子 中 包含 的 信息 很 少 。 换 句 话 
说 ,就 是 直接 将 粒子 抽样 推广 到 高 似 然 度 区 域 。 

以 下 为 关于 APF 的 一 些 论述 : 

(1) 在 一 般 SIS 粒子 滤波 器 中 ,通常 是 在 重 采 样 步 又 之 后 进行 估计 ,这 样 效 
率 很 低 ,因为 重 采 样 引 入 了 额外 的 当前 状态 的 随机 变化 。APF 通过 一 步 基 于 点 
估计 js ”表示 p(x, |x)” ) 的 预 估计 克服 了 这 一 问题 。 

(2) 当 过 程 噪声 很 小 时 ,APF 的 性 能 通常 优 于 SIS 滤波 器 。 然 而 , 当 过 程 品 
声 很 大 时 ,点 估计 Jp, 无 法 提供 足够 的 p(x, 1," ) 信 息 ,这 样 就 无 法 保证 APF 的 
优势 。 ' 

(3) fE APF 中 ,重要 性 密度 建议 为 混合 密度 ,依赖 于 过 去 的 状态 和 最 近 
观测 值 。 

(4) APF 的 思想 与 文献 [18] 中 提出 的 局 部 蒙特 卡 洛 方法 相同 ,作者 给 出 两 
种 方法 实现 基于 联合 分 布 或 边缘 分 布 的 |x,é| 样 本 抽样 。 

(5) APF 的 缺点 是 从 增 大 的 ( 因此 更 高 的 ) 空间 中 进行 样本 抽样 ,如 果 辅 助 
索引 与 固定 先 验 相 比 差异 很 大 ,这 种 增 大 可 以 忽略 ,重要 性 权 值 的 方差 会 更 高 。 


16.5.1 APF 在 DIFAR 浮标 跟踪 中 的 应 用 


在 DIFAR 实例 中 ,采用 APF 作为 跟踪 滤波 器 ,希望 跟踪 结果 与 高 斯 卡尔 曼 
滤波 器 相 比 能 有 所 改善 。 但 如 图 16 -6 所 示 , 对 6 个 信 品 比 递减 的 输入 信号 进 
行 蒙 特 卡 洛 跟踪 结果 进行 对 比 ,APF 并 没有 表现 出 任何 优势 。 造 成 这 种 现象 的 
原因 是 显而易见 的 。 粒 子 滤波 器 是 在 当 后 验 密度 (表示 动态 模型 ) 为 非 高 斯 时 
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实现 估计 。 但 是 对 于 DIFAR 实例 ,动态 模型 为 线性 高 斯 而 似 然 函数 是 非 高 斯 
的 。 因 此 ,在 这 种 情况 下 ,APF 并 不 优 于 高 斯 卡尔 曼 滤波 器 。 
4 5 
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` 400 -500 
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16 -6 将 辅助 粒子 滤波 器 应 用 于 DIFAR 跟踪 实例 的 跟踪 估计 结果 


16.6 序 贯 重要 性 采样 辅助 粒子 滤波 器 估计 


下 面 给 出 另 一 种 SIS 粒子 滤波 方法 ,与 上 面 给 出 的 APF 非常 类 似 。 假 设 用 
高 斯 密度 估计 得 后 验 密度 plx, lz.) 和 最 优 重要 性 密度 g(x? |x ,z,) ,后 验 
密度 估计 为 


p(x,| z...) = N (x,;x,,P,) (16 -66) 
用 高 斯 建议 分 布 来 近似 每 个 粒子 的 建议 分 布 , 即 
人 (16 -67) 


即 在 4,_, 时 刻 , 利 用 本 书 第 工 部 分 中 给 出 的 高 斯 卡尔 曼 滤波 器 的 变 体 ,根据 新 的 
ALME ,计算 每 个 粒子 重要 性 分 布 的 均值 和 协 方差 。 然 后 ,在 这 个 分 布 中 进行 第 
i 个 粒子 的 采样 。 


16.6.1 扩展 卡尔 曼 粒 子 滤 波 器 


在 Merwe 等 ”首次 提出 的 扩展 卡尔 曼 粒 子 滤波 器 中 ,将 非 线性 动态 方程 和 
观测 方程 展开 为 泰勒 级 数 ,并 应 用 于 EKF 当中 ,以 生成 第 i 个 粒子 的 前 两 阶 矩 
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(xi? (PO) ,将 其 用 在 式 (16 - 67) 中 来 定义 高 斯 建议 分 布 。 然 后 ,从 建议 分 布 中 
抽取 第 i 个 样本 , 即 


&Ó = gle") ae) = Nt PO) (16 -68) 
HiO FRA SR (16 -12) 来 更 新 权重 , 即 
(i) x (i) 
wi) “< wo PCa | $, DPC | xs) (16 -69) 


q(x! x” | x" Za) 


这 样 就 得 了 规范 化 权 值 。 因 为 这 个 方法 与 前 面 提 出 的 高 斯 最 优 SIS 粒子 滤波 器 几 
乎 完全 相同 ,读者 可 自行 参阅 文献 [19 ] ,以 获得 此 方法 相关 具体 步骤 的 详细 信息 。 


16.6.2 无 迹 粒 子 滤波 器 


文献 [19] 提 出 了 无 迹 粒 子 滤波 器 (UPF) ,对 于 高 度 非 线 性 系统 UKF 已 被 证 
实 比 卡尔 曼 滤波 器 更 为 精确 。 用 UKF 取代 EKF ,生成 用 于 高 斯 建议 分 布 的 前 两 
ME, UPF 方法 非常 简单 .容易 实现 , 表 16 -7 给 出 了 详细 步骤 ,可 以 用 表 13 -2 
来 计算 Sigma 点 权重 。 此 外 , 表 中 省 略 了 一 些 与 前 面 的 粒子 滤波 器 完全 相同 的 
步骤 。UPF 相关 信息 可 参阅 文献 [20 ,21 ] ,UPF 的 平方 根 变 体 可 参阅 文献 [22] 。 
16-7 带 重 采样 和 正则 化 的 无 迹 粒 子 滤 波 器 
1. 滤波 器 初始 化 
(1) 初始 化 状态 矢量 样本 ; oq) 
(2) 初始 化 权 值 


2. 重要 性 采样 
For i = 1,---,N, 


(1) 计算 第 j 个 Sigma 点 ; OP = FO + (Pe r j=0,1,.,2n, 


1 - wg 


2n, 


ZG še a (iy) 
X in-l 一 R Pacha 
' 


(2) 状态 预测 


2n, 
PEO! = >u fO: l. (y. )-x0)_ zb +Q 


xi, = 29), + J = woe ra Or?) j20,1,,2n, 


2n. 


zü i) - w.h Qt /) 1) 
P Ue 
PK ^ = Y. wh rii Ms aT QST, a) - 20 eR) | +R 


Aes Yn fers MA Qua - 390129), 





(5) 
. 更 新 卡尔 曼 滤 波 器 KÖ = pO [P0] 
x) = HHE - Ki" (z, - zi ) 
Pi a Mj RI MED CO 
p(z, | xf? p(x | xl) 


WE) . (a) 
Ww, = w iol 


£) | (i) : 
. 更 新 重要 性 权 值 q(x, | YXn-1vZn) 


6. 重 采 样 并 移动 
. 矩 计算 
. 时 间 递 增 , 返 回 第 2 步 


注意 :UKF 只 是 可 用 于 此 类 应 用 的 Sigma 点 卡尔 曼 滤波 器 类 中 的 一 种 。 可 
以 很 容易 地 提出 一 种 球面 单 形 粒子 滤波 器 或 者 高 斯 - JU KE SIS 粒子 滤波 器 ， 
将 这 些 滤波 器 的 权 值 和 Sigma 点 代入 表 16 -7 的 运算 步骤 中 。 实 际 上 ,对 于 高 
维 状态 矢量 ,用 球面 单 形 卡尔 曼 滤波 器 来 代替 无 迹 卡尔 曼 滤波 器 是 一 个 好 思路 。 
男 一 方面 ,对 于 低 维 状态 矢量 和 高 度 非 线 性 模型 , 换 成 高 斯 — 厄 密 特 卡尔 曼 滤波 
器 会 更 有 意义 。 为 保证 完整 性 ,甚至 可 以 在 这 种 应 用 中 使 用 蒙特 卡 洛 卡尔 曼 滤 
波 器 ,这 样 不 会 损失 任何 一 般 性 。 








16.7 利用 Rao - Blackwellization 理论 降低 计算 负荷 


在 许多 状态 估计 的 应 用 中 ,状态 矢量 是 高 维 的 ,SIS 粒子 滤波 器 的 计算 负荷 
变 得 极 大 ,估计 精度 可 能 迅速 降低 。 当 应 用 于 高 维 状态 空间 时 ,因为 需要 大 数量 
的 样本 来 保证 密度 函数 的 估计 的 精确 性 ,基于 粒子 滤波 的 方法 的 效率 变 得 极 低 。 
状态 矢量 的 分 量 可 以 细 分 为 两 类 :一 类 遵循 线性 时 域 迁移 ,是 高 斯 的 ;第 二 类 是 
非 高 斯 的 ,可 利用 Rao - Blackwellization ^ -来 减少 计算 负荷 。 
Rao - Blackwell 算法 的 关键 思想 是 降 维 , 利 用 模型 结构 将 条 件 后 验 分 成 两 
个 部 分 ,其 中 高 斯 部 分 以 非 高 斯 部 分 为 条 件 。 首 先 用 粒子 滤波 器 生成 非 高 斯 后 
验 ,然后 对 条 件 高 斯 部 分 应 用 卡尔 曼 滤波 器 。 假 设 状态 矢量 分 量 可 以 被 分 成 丙 
Hl x, =[g",r"]", 这 样 有 
p(x,!z,,) = p(q,,r, 1 z.) = PCA l r,,z, ,)p(r,.! Zia) (16 -70) 
在 文献 [24] 中 指出 , 当 q, 的 初始 不 确定 性 为 高 斯 分 布 ,而 q, 的 观测 模型 和 系统 动 


态 的 条 件 概 率 也 为 高 斯 分 布 时 ,可 以 用 线性 卡尔 曼 滤波 器 有 效 地 更 新 p(g,1r,， 
zi ) 。 为 深入 理解 并 使 用 Rao - Blackwell 方法 ,读者 可 自行 参阅 文献 [3 -24 -25]. 
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第 17 Ë 
广义 蒙特 卡 洛 粒子 滤波 器 





所 有 SIS 类 粒子 滤波 器 都 是 基于 式 (16 - 12) ,该 式 定 义 了 重要 性 权 值 的 递 
归 更 新 。 将 递归 重要 性 权 值 更 新 与 重 采 样 和 移动 步 又 相 结合 ,提供 了 一 个 非 线 
性 非 高 斯 系统 的 估计 方法 框架 ,只 需要 选择 重要 性 密度 和 似 然 函 数 。 但 是 ,SIS 
粒子 滤波 器 在 使 用 中 有 许多 缺点 : 

(1) BPF 在 重要 性 采样 过 程 中 不 使 用 最 新 的 测量 值 ,使 得 它 容 易 受 许多 应 
用 中 方差 和 不 稳定 性 增加 的 影响 。 

(2) 所 有 SIS 粒子 滤波 器 都 需要 重 采样 和 移动 步骤 ,来 增加 估计 状态 误差 
协 方 差 。SIS 粒子 滤波 器 不 能 并 行 化 ,因此 还 需要 增加 一 个 重要 的 计算 负荷 估 
计 步 又 。 

(3) SIS APF 及 其 替代 算法 可 能 需要 对 每 个 粒子 在 每 个 时 间 步 执行 高 斯 卡 
尔 曼 滤波 器 ,进一步 提高 了 计算 负荷 。 

第 二 类 粒子 滤波 器 是 广义 蒙特 卡 洛 (GMC ) 粒子 滤波 器 , 它 不 使 用 递归 的 权 
值 更 新 ,而 是 在 每 次 序 贯 更 新 滤波 器 时 计算 新 的 权 值 。 这 类 更 为 通用 的 粒子 滤 
波 器 的 实例 包括 Kotechha 和 Djuric "提出 的 高 斯 粒子 滤波 器 。 


17.1 高 斯 粒子 滤波 器 


再 次 回顾 贝 叶 斯 估计 的 基本 方程 , 记 住 这 是 一 个 两 步 过 程 ,滤波 步骤 由 后 验 
估计 (滤波 或 更 新 ) 密度 得 出 , 即 
P(X, | Zin) = ep(z, | x,)p(x, | zia) (17 — 1) 
预测 步骤 包括 预测 密度 (或 先 验 密度 , 因为 它 使 用 之 前 的 所 有 测量 值 ) 的 估 
计 , 即 
P(X, | zi:n-1) = pe, | x, )p(x,. | Zin- ) dx (17 -2) 
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GPF 的 基本 概念 是 假设 后 验 密 度 和 先 验 密度 都 是 高 斯 分 布 , 即 


p(x, | Zin) = N(x, ut, By) (17 -3) 
P(X, | Zina) = N(x, ut, Dy) (17 -4) 

假设 在 i。 时刻, 在 所 有 观测 值 之 前 ,有 如 下 初始 密度 的 先 验 信息 , 即 
p(x,) = N( Xo uas Bo) (17 -5) 


为 初始 化 CPF ,首先 由 P(xo) WEER Lx 5° 17 ,然后 使 用 动态 方程 将 样本 在 时 
间 上 向 前 传播 , 即 


© ast DIME ins) +E, (17 -6) 
然后 计算 万 和 马 , 即 
N 
一 l € «ii 
Hn, = N à (17 —7) 
ass l N, 
B= 2 unas ani nMn + Q (17 - 8) 
si=lnln-1 


这 定义 了 式 (17 - 4) 的 先 验 (预测 ) 密度 。 
回顾 式 (15 - 41) ,可 以 将 重要 性 权 值 计算 方 法 写成 非 迭 代 的 形式 , 即 
p(z, | x,) N(x,; p, X.) 
w, x ———————MÁáMML 
Ë q(x, | Z=.) 
在 从 ( 当前 未 指明 的 ) 重要 性 密度 中 采样 后 ,x;” ~ g(x, E zu). = 1, N,EX 
值 变 成 


(17 -9) 





17 — 10 
q(x, = x” | Zin) ( ) 
经 过 规范 化 步骤 ,得 
~ (i) 
wi? = (17 - 11) 
^v (i) 
可 以 计算 后 验 密度 的 矩 BI 
X, = y wx (17 - 12) 
i=1 

N, 
Pu = yw: 2 (17 - 13) 

i=l 


这 样 就 完成 了 高 斯 粒子 滤波 器 过 程 ,如 图 17 -1 所 示 。 注 意 ,广义 蒙特 卡 洛 GPF 
仍然 需要 额外 的 似 然 函数 和 重要 性 密度 的 解析 表达 式 。 
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图 17 -1 高 斯 粒子 滤波 器 过 程 流 图 


17.2 ”组合 粒子 滤波 器 


图 17 - 1 并 未 指明 重要 性 密度 。 因 此 ,GPF 是 不 完整 的 ,需要 指定 一 个 重要 
性 密度 ,在 文献 [1] 中 ,建议 采用 高 斯 分 布 作为 重要 性 密度 , 即 令 q(x, 1 zi:,) = 
NX, Mains Dan) ,其 中 jm, AS, FAAS SS I 部 分 的 任意 一 种 非 线性 卡尔 曼 滤 
波 器 ,由 先 验 密度 求解 ,这 种 方法 将 GPF 与 一 种 非 线 性 卡尔 曼 滤波 器 结合 起 来 ， 
因此 称 为 广义 蒙特 卡 洛 组 合 粒子 滤波 器 (CPF ) 。 

可 以 将 CPF 划分 为 两 类 :利用 EKF 来 生成 重要 性 密度 和 利用 Sigma 点 卡尔 
曼 滤波 器 .Sigma 点 卡尔 曼 滤波 器 类 可 以 进一步 分 成 4 种 数值 积分 卡尔 曼 滤 波 
器 :MCKF „UKF SSKF 或 CHKF.。 图 17 -2 给 出 了 CPF - EKF 估计 滤波 器 递归 过 
程 流 图 ,图 17 -3 给 出 了 利用 Sigma 点 卡尔 曼 滤波 器 的 CPF 过 程 流 图 。 在 图 的 下 
端 可 看 到 高 斯 粒子 滤波 器 的 结构 ,而 在 图 的 右 侧 ,可 以 看 到 非 线性 卡尔 曼 滤波 器 
结构 ,在 参考 文献 [3] 中 ,提出 一 种 类 似 的 无 迹 粒 子 滤波 器 ,但 是 不 包括 先 验 的 
高 斯 估计 。 需 要 注意 文献 [3] 包含 大 量 的 错误 ,使 它 非常 难以 遵循 。 

广义 蒙特 卡 洛 组 合 粒子 滤波 器 在 多 数 应 用 中 运行 良好 ,这 是 因为 与 重要 性 
密度 相关 的 方差 通常 比较 宽 , 包 括 后 验 密度 支撑 的 主要 部 分 ,除了 后 验 为 重 尾 分 
布 的 应 用 ,对 于 任何 应 用 来 说 ,组 合 粒子 滤波 器 是 近似 "最 优 ” 的 粒子 滤波 器 。 因 
为 组 合 粒 子 滤波 器 不 需要 采样 和 移动 步骤 ,而 且 每 次 迭代 仅 执 行 一 次 非 线 性 卡 
尔 曼 滤波 器 ,计算 效率 显著 提高 , 优 于 大 部 分 的 SIS 粒子 滤波 器 。 

对 先 验 后 验 密度 重 尾 或 多 峰 情 况 , 有 多 种 备 选 方案 ,可 以 用 来 提高 广义 蒙 
特 卡 洛 粒子 滤波 器 类 的 性 能 。 针 对 多 峰 非 高 斯 密度 ,Kotecha 和 Djuric*' 提出 一 
种 高 斯 混合 粒子 滤波 器 ,其 中 预测 和 滤波 分 布 均 近似 高 斯 混合 。 在 许多 应 用 中 ， 
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图 17 -2 利用 EKF 作为 重要 性 密度 的 组 合 粒 子 滤波 器 
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图 17 -3 利用 Sigma 点 卡尔 曼 滤 波 器 作为 重要 性 密度 的 组 合 粒子 滤波 器 
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这 些 高 斯 求 和 粒子 滤波 器 (Gaussian Sum Particle Filters, GSPF) 提高 了 性 能 。 
Merwe Fil Wan 在 文献 [5] 中 提出 一 种 Sigma 点 高 斯 混合 组 合 粒 子 滤波 带 ,根据 受 
y 噪声 影响 的 高 度 非 线 性 动态 转移 方程 估计 一 维 状 态 , 带 动态 噪声 的 状态 和 失 量 
得 增强 ,即便 在 重 尾 y 分 布 导 致 很 多 异常 观测 值 的 情况 下 ,其 结果 也 非常 可 靠 。 

可 以 用 一 些 广义 蒙特 卡 洛 方法 的 改进 方法 ,来 处 理 重 尾 噪声 分 布 - 例 如 :使 
用 Levy 分 布 .alpha — stable 分 布 或 拉 普 拉 斯 分 布 来 取代 高 斯 逼近 作为 预测 和 滤 
波 密度 ;用 Kalman - Levy 滤波 器 广义 stable 滤波 器 '" 或 者 拉 普 拉 斯 滤波 
器 ”取代 非 线 性 高 斯 卡尔 曼 滤波 器 ,这 些 滤波 器 具有 与 卡尔 曼 滤波 器 类 似 的 对 
称 结构 ,很 容易 用 来 在 组 合 粒 子 滤波 器 应 用 中 代替 非 线 性 高 斯 卡尔 曼 滤 波 器 。 
Kalman - Levy 滤波 器 和 拉 普 拉 斯 滤波 器 "| 已 成 功用 作 机 动 目标 跟踪 滤波 
器 ,但 尚未 被 用 在 粒子 滤波 器 应 用 中 。 

和 高 斯 卡尔 曼 滤波 器 类 似 , 所 有 这 些 方法 需要 一 致 的 密度 解析 形式 .所 有 的 
滤波 器 只 使 用 一 个 解析 密度 .Chen 和 Singpurwalla" 提出 了 另 一 种 广义 非 线性 
卡尔 曼 滤波 器 结构 ,其 中 解析 密度 会 根据 滤波 器 的 不 同 部 分 而 变化 ,对 于 滤波 器 
的 不 同 部 分 ,包括 状态 预测 ,观测 值 预测 和 卡尔 曼 滤波 器 更 新 形式 ,利用 y 分布 、 
B 分 布 和 Pearson 分 布 ,始终 坚持 更 新 卡尔 曼 滤波 器 的 公式 表示 ,在 实际 的 应 用 
中 ,使 用 第 12 章 中 给 出 的 一 种 与 MCKF 相似 的 滤波 器 ,其 方法 可 以 很 容易 应 用 
在 广义 粒子 滤波 器 形式 上 。 


17.2.1 CPF - UKF 在 DIFAR 浮标 跟踪 中 的 应 用 
图 17 -4 给 出 6 种 信 噪 比 下 CPF - UKF 应 用 于 DIFAR 跟踪 问题 得 到 的 蒙 
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图 17 -4 6 种 信 噪 比 下 Sigma 点 高 斯 滤波 器 航 迹 图 
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特 卡 洛 跟踪 结果 .与 之 前 其 它 跟 踩 滤波 器 进行 比较 ,可 以 看 出 这 种 滤波 器 并 不 优 
于 (也 不 劣 于 ) 其 它 任 何 滤波 器 。 


17.3 DIFAR 跟踪 滤波 器 的 性 能 对 比 


本 节 将 一 些 跟踪 滤波 器 应 用 于 DIFAR 实例 ,并 分 析 其 性 能 ,本 书 把 这 种 性 
能 分 析 放 到 在 第 H 和 第 亚 部 分 介绍 完 所 有 的 滤波 器 之 后 ,因为 到 目前 为 止 ,所 
有 的 结果 都 是 基于 仿真 观测 数据 ,而 最 好 的 性 能 估计 方法 是 比较 不 同 滤波 器 的 
x 坐标 位 置 和 y 坐标 位 置 的 RMSE。 对 于 每 种 信号 信 唆 比 ,给 出 4 种 高 斯 滤波 器 
(EKF UKF SSKF fil GHKF) 和 3 种 粒子 滤波 器 (BPF APF 和 GSPF) 的 RMS 位 
置 误差 比较 图 .CSPF 将 粒子 滤波 器 与 UKF 组 合作 为 重要 性 密度 -对 于 所 有 的 粒 
子 滤波 器 ,使 用 3000 个 蒙特 卡 洛 样本 。 

作为 第 一 个 实例 ,图 17 -5 给 出 信号 信 噪 比 为 204B 时 ,将 x* 轴 和 y 轴 均 方 根 
误差 作为 目标 船舶 的 真实 位 置 函数 的 比较 ,可 以 看 到 ,与 图 16 - 5 一 样 ,在 信 品 
比 为 20dB 时 ,由 于 有 3 个 发 散 航 迹 ( 从 100 次 蒙特 卡 洛 运行 中 得 ) ,BPF 的 RMS 
位 置 误差 迅速 增加 。 这 只 是 关于 BPF 初 值 敏感 性 的 男 一 个 例证 ,因为 所 有 的 滤 
波 器 都 采用 同一 组 初 值 进行 初始 化 ,BPF 所 需 的 这 种 额外 的 调整 对 于 真实 世界 
跟踪 应 用 来 说 ,是 不 可 能 实现 的 。 
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图 17 -5 当 输 入 信号 信 噪 比 为 20dB 时 ,DIFAR 实例 均 方 根 位 置 误差 比较 


注意 ,所 有 的 高 斯 卡尔 曼 滤波 器 变 体 的 性 能 都 几乎 相同 , 略 优 于 APF 和 
GSPF。 前 面 章 节 已 暗示 ,在 所 有 粒子 滤波 器 中 ,GSPF 的 性 能 最 好 。 图 中 还 可 以 
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看 出 ,大 多 数 滤波 器 在 跟踪 刚刚 开始 时 RMSE 跟踪 性 能 指标 最 高 , 当 船 舶 驶 人 
浮标 场 时 达到 最 小 值 ,几乎 是 关于 原点 对 称 的 ,最 初 的 高 RMSE 是 由 于 滤波 器 
初始 值 选取 得 不 好 ,但 当 滤 波 器 综合 一 些 观测 值 之 后 ,就 可 以 回归 到 合理 的 跟 
踪 估 计 。 

如 图 17 - 6 所 示 , 当 信号 信 噪 比 降 至 15dB 时 ,相对 于 20dB 的 情况 ,RMS 位 
置 误差 增加 。 由 于 在 这 个 信 噪 比 下 有 7 个 发 散 航 迹 , 所 以 BPF 的 位 置 RMSE 是 发 
散 的 ,在 100 次 蒙特 卡 洛 运行 中 ,其 它 滤 波 器 都 没有 任何 发 散 航 迹 。 在 高 信 噪 比 下 ， 
高 斯 滤波 器 的 RMS 位 置 误差 与 APF 和 GSPF 相 比 几乎 相同 , 稍 高 一 点 .图 14 -3 至 
图 14 -7 给 出 信号 信 噪 比 由 20dB 降 至 0dB 时 高 斯 滤波 器 的 RMS 位 置 误差 ,这 些 
图 中 的 高 斯 滤波 器 的 RMS 误差 均 按 同一 比例 ,因此 可 以 更 好 地 比较 高 斯 滤波 器 
误差 ,也 能 表明 高 斯 滤波 器 之 间 细 微 的 性 能 差异 。 
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图 17 -6 当 输 入 信号 信 噪 比 为 13dB 时 ,DIFAR 实例 均 方 根 位 置 误差 比较 


图 17 -7 给 出 了 10dB 信号 的 RMS 位 置 误差 ,在 这 个 信号 水 平 ,APF 有 4 个 
发 散 航 迹 (参见 图 16 -6) ,导致 RMS 误差 增加 .但 GSPF 和 所 有 高 斯 滤波 器 仍然 
很 稳定 ,这样 它们 的 RMS 误差 仍然 是 可 以 接受 的 。 这 里 不 会 给 出 信 噪 比 低 于 
10d4B 时 的 RMS 位 置 误 差 图 ,因为 这 时 所 有 滤波 器 的 蒙特 卡 洛 跟踪 估计 都 出 现 一 
些 发 散 航 迹 , 导 臻 RMS 误差 计算 无 效 。 

根据 上 述 性 能 分 析 以 及 第 14 章 结尾 所 指出 的 ,只 有 当 信 噪 比 大 于 5dB 时 ， 
才能 够 实现 穿 过 DIFAR 浮标 场 船 舶 的 跟踪 ;否则 这 些 观 测 值 没 有 利用 价值 .可 
能 有 读者 会 认为 还 需要 更 多 的 粒子 来 提高 粒子 滤波 器 的 性 能 ,但 将 粒子 的 数目 
由 1000 增加 到 3000 时 ,性 能 几乎 没有 改善 - 综 上 所 述 ,BPF 对 初始 值 非常 敏感 ， 
使 用 时 需要 特殊 注意 。 
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图 17 -7 HAA o RECS 10dB 时 ,DIFAR 实例 均 方 根 位置 误 差 比 较 
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第 IV 部 分 
附加 案例 分 析 


第 18 章 ， 用 于 三 维 目标 跟踪 的 球 坐 标 恒 速度 模型 
第 19 章 利用 摄影 测量 法 跟踪 下 落 刚 体 
第 20 & ”用 摄影 测量 和 惯性 测量 进行 传感器 融合 


第 18 章 
用 于 三 维 目标 跟踪 的 球 坐 标 恒 速度 模型 





这 个 案例 致力 于 解决 一 个 非常 重要 的 估计 问题 ,也 是 在 许多 工程 应 用 的 核 
心 问 题 —— 三维 空间 物体 跟踪 。 许 多 三 维 跟踪 算法 利用 惯性 坐标 系 下 的 笛 卡 儿 
恒 速 度 线性 动态 模型 来 估计 目标 运动 ,并 用 非 线性 观测 模型 将 笛 卡 儿 坐 标 状 态 
矢量 x 与 一 组 球 坐 标 观测 值 z 联 系 起 来 ,如 果 将 动态 噪声 和 观测 噪声 设 为 高 斯 分 
布 ,执行 卡尔 曼 滤波 器 得 一 组 线性 卡尔 曼 滤波 器 状态 预测 方程 和 非 线 性 观测 预 
测 方程 ,可 以 利用 本 书 第 [部 分 提出 的 任何 一 种 次 最 优 滤波 技术 对 方程 进行 
求解 。 

已 经 提出 多 种 方法 ,来 研究 解决 机 动 (加 速 ) 目标 跟踪 问题 .对 于 笛 卡 儿 坐 
标 跟 踪 ,最 简单 的 方法 是 在 目标 动态 模型 中 去 掉 恒 速度 的 约束 , 即 通过 在 状态 矢 
量 中 加 入 加 速度 分 量 ,以 及 用 恒 加 速度 约束 "'“! 取代 速度 恒定 约束 。 当 目标 只 有 
很 短 的 时 间 是 机 动 的 ,大 部 分 的 航 迹 上 都 是 非 机 动情 况 下 ,这 种 算法 会 出 现 问 
题 , 为 处 理 这 类 情况 ,一些 学 者 提出 ,在 检测 到 机 动 性 时 使 用 滤波 器 ,从 恒 速 度 模 
型 切换 到 恒 加 速度 模型 ,但 机 动 性 的 检测 并 不 容易 .文献 [2 - 4] 给 出 另 一 种 方 
法 ,该 方法 使 用 很 多 模型 ,每 个 模型 用 来 跟踪 不 同 的 潜在 的 机 动 目标 ,还 有 另 一 
种 模型 由 交互 多 模型 (Interactive Multiple Model,IMM) 构成 ,其 中 使 用 一 族 滤 
波 器 模型 ,模型 间 只 有 动态 噪声 有 所 不 同 ,其 输出 构成 了 混合 分 布 。 对 IMM 算法 
感 兴趣 的 读者 可 参考 上 述 文献 及 文献 中 包括 的 参考 文献 。 

尽管 上 述 机 动 目标 跟踪 方法 对 于 一 些 场景 跟踪 效果 很 好 ,一 些 学 者 也 提出 
了 其 它 的 方法 ,将 目标 动态 模型 转换 至 球 坐 标 系 ,以 保持 惯性 笛 卡 儿 坐 标 系 零 加 
速度 的 约束 ,在 文献 [5 —8] 提出 的 方法 中 ,一 组 正 交 笛 卡 儿 坐 标 轴 相对 于 惯性 
坐标 系 进行 旋转 ,这 样 始终 有 一 个 轴 与 连 线 轴 (距离 轴 ) 对 齐 。 状 态 矢量 在 旋转 
的 球 坐 标 系 中 ,由 于 旋转 坐标 系 产 生 科 里 奥 利 力 ( Coriolis forces) ,必须 将 伪 加 
速度 引入 到 动态 运动 方程 中 。 伪 加 速度 加 上 惯性 坐标 系 笛 卡 儿 恒 速 度 的 假设 ,得 
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一 组 非 线性 完全 耦合 的 球 坐 标 动 态 方程 . 当 应 用 于 固定 的 惯性 坐标 系 ” 下 跟踪 
机 动 目标 时 ,这 些 滤波 器 的 性 能 很 低 。 

本 章 将 提出 一 种 更 为 简单 的 目标 动态 模型 ,适用 于 沿 着 机 动 和 非 机 动 航 迹 
跟踪 目标 。 该 模型 用 惯性 坐标 系 球 坐 标 恒 速度 约束 取代 惯性 坐标 系 笛 卡 儿 坐 标 
恒 速 度 约束 ,其 中 限制 正 交 球速 度 分 量 为 常数 ,虽然 机 动 目标 经 常会 违反 恒 速 度 
约束 ,但 试验 表明 ,在 机 动 过 程 中 , 球 坐 标 约束 下 的 RMS 跟踪 误差 比较 小 ,文献 
[5] 在 以 前 就 给 出 了 类 似 的 结果 。 

本 章 后 面部 分 将 进行 笛 卡 儿 坐 标 恒 速度 假设 与 球 坐 标 恒 速 度假 设 跟 踪 算 法 
性 能 的 比较 ,因此 ,从 第 18. 1 节 开 始 研究 在 笛 卡 儿 坐 标 系 下 目标 跟踪 ,这 里 假定 
第 卡 儿 速 度 恒定 ,然后 给 出 动态 方程 和 观测 方程 ,并 阐述 了 第 卡 儿 坐标 系 下 的 扩 
展 卡尔 曼 滤波 器 和 Sigma 点 卡尔 曼 滤波 器 。 

第 18.2 节 提 出 一 种 新 的 非 线性 恒定 球速 度 动态 方程 和 线性 观测 方程 ,这 是 
根据 第 一 原则 导出 的 ,因为 动态 方程 是 指 一 个 惯性 不 旋转 坐标 系 ,方程 中 不 会 出 
现 伪 加 速度 。 基 于 这 个 模型 ,给 出 了 球 坐 标 扩展 卡尔 曼 滤 波 器 和 Sigma 点 卡尔 曼 
滤波 器 .对 于 远 距 离 情况 ,可 将 一 般 的 非 线性 球 坐 标 动态 方程 化 简 为 一 组 线性 恒 
定 角 速率 动态 方程 ,得 更 为 简单 的 球 坐 标 线性 卡尔 曼 滤波 器 。 

第 18. 3 节 讨 论 了 滤波 器 实现 问题 ,第 18.3. 1 节 起 始 部 分 描述 了 动态 噪声 项 
gq( 后 面 将 给 出 其 定义 ) 的 分 量 的 含义 ,以 及 如 何 根据 预期 目标 加 速度 限制 常识 
来 设置 它们 的 值 ,所 有 跟踪 滤波 器 都 需要 两 组 必 不 可 少 的 数据 集合 作为 输入 : 观 
测 值 集合 和 初始 值 集合 ,第 18.3.2 节 说 明 如 何 生 成 一 个 理论 上 的 雷达 的 观测 
值 , 供 第 18.4 节 中 所 有 的 性 能 评估 使 用 ,第 18. 3. 3 节 讨 论 了 根据 最 开始 很 少 的 
含 品 观测 值 生成 初 值 的 方法 。 

第 18.4 节 比 较 了 第 卡 儿 坐 标 系 滤波 器 和 球 坐 标 系 滤波 器 的 性 能 ,通过 对 目 
标 机 动 水 平 逐 渐 增 加 的 3 个 仿真 航 迹 的 性 能 分 析 , 可 以 看 到 , 当 目 标 为 机 动 的 且 
目标 距离 接近 于 零 时 , 球 坐 标 跟踪 估计 滤波 占有 更 好 的 性 能 。 

第 18. 5 节 将 给 出 这 个 案例 的 一 些 观测 值 ,并 对 上 述 工作 未 来 可 能 的 发 展 方 
向 进行 了 展望 。 

本 章 结尾 给 出 两 个 附录 ,它们 对 于 理解 三 维 跟踪 的 发 展 非常 重要 .因为 仿真 
实例 贯穿 全 章 , 附 录 18. A 讨论 了 在 三 维 空间 中 生成 机 动 目标 航 迹 的 方法 .通常 
是 通过 使 用 高 度 保 真 的 6 自由 度 (6DOF ) 仿真 机 动 对 象 ,利用 一 个 完整 的 物体 作 
用 力 模型 ,如 推力 5855€ 71 .重力 等 ,来 实现 目标 跟踪 。 因 为 本 书 只 对 不 同 跟踪 算 
法 的 之 间 的 比较 感 兴趣 ,在 附录 18. A 中 给 出 一 个 低 精 度 的 物体 运动 仿真 模型 , 
该 模型 只 能 简单 地 对 可 能 的 机 动 性 进行 建 模 ,通过 将 恒定 的 水 平和 垂直 转弯 速 
率 结合 起 来 ,基于 期 望 的 转弯 / 俯冲 加 速度 得 逼真 的 航 迹 。 利 用 这 个 恒定 转弯 速 
率 解 析 模 型 ,生成 3 个 航 迹 实例 ,实例 中 机 动 目标 的 数目 不 断 增 多 。 第 18. 4 节 利 
用 这 些 航 迹 在 目标 机 动 数量 增 加 时 ,评估 跟踪 器 的 性 能 。 
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当 采 用 跟踪 算法 在 三 维 空间 跟踪 一 个 物体 时 ,可 以 选择 坐标 系统 ,通常 是 笛 
卡 儿 坐标 系 或 者 球 坐 标 系 。 由 于 跟踪 应 用 中 使 用 的 多 数 传感器 , 例如 雷达 或 声 
纳 , 所 得 的 测量 值 都 是 球 坐 标 系 下 的 ,因此 6 维 (3 个 位 置 和 3 个 速度 分 量 ) 坐标 
变换 子 程序 是 必 不 可 少 的 工具 。 多 数 坐 标 变换 子 程序 只 处 理 状态 矢量 变换 ,然而 
许多 跟 踩 算法 需要 协 方差 矩阵 坐标 变换 或 需要 计算 雅 可 比 矩 阵 , 附 录 18. B 给 出 
完整 的 坐标 变换 : 笛 卡 儿 坐标 系 到 球 坐 标 系 变换 、 球 坐标 系 到 笛 卡 儿 坐 标 系 变换 
以 及 协 方差 矩阵 的 计算 。 作 为 协 方差 变换 的 附带 结果 ,也 会 产生 雅 可 比 矩 阵 。 因 
为 在 分 析 中 使 用 向 东 — 向 北 — 向 上 (x,y,z 轴 正 方向 ) 笛 卡 儿 坐 标 系统 ,也 将 介 
绍 在 这 个 坐标 系统 的 变换 。 最 后 提出 了 一 种 通用 方法 ,让 读者 推导 出 任意 三 维 坐 
标 系 统 下 的 这 种 变换 。 


18.1 币 卡 儿 坐 标 系 下 目标 跟踪 


本 节 讨 论 使 用 定义 在 笛 卡 儿 坐 标 系 下 的 状态 矢量 的 进行 目标 跟踪 的 方法 。 
本 节 分 为 若干 小 节 , 讨 论 用 动态 模型 来 定义 预期 对 象 运动 (18.1.1 节 ) UM A 
量 及 其 如 何 与 状态 矢量 相关 (18. 1.2 r) 还 有 多 种 可 以 用 于 解决 这 个 问题 的 笛 
卡 儿 高 斯 滤波 器 跟踪 算法 (18. 1. 3 节 ) 。 滤 波 器 初始 化 和 生成 蒙特 卡 洛 观测 值 集 
合 的 讨论 将 会 放 到 第 18.3 T. 


18.1.1 目标 动态 运动 模型 


在 笛 卡 儿 坐 标 系 下 ,用 一 组 静止 雷达 来 跟踪 运动 物体 .假设 物体 以 恒定 速度 
运动 ,可 得 如 下 形式 的 线性 动态 模型 , 即 


S = F. K... 945 (18 — 1) 
t, 时 刻 的 笛 卡 儿 坐 标 系 状 态 矢量 定义 为 
x, =x(t,) = [5 of, kh f S (18 - 2) 
转移 矩阵 F, 定义 为 
Fin 0, 0, 
mp Eia ^| (18 -3) 
0, 0, F,, 
t T 
F,„ = p iy (18 - 4) 


0, 为 一 个 2 x 2 WREE, T, 定义 为 
f E k. (18 —5) 
考虑 到 传感器 跟踪 的 时 间 异 步 性 质 ,可 将 也, 设 为 时 间 变 量 。 然 而 ,在 后 续 分 析 
中 ,只 用 到 单个 传感器 的 情况 ,所 以 将 了 设 为 一 个 不 随时 间 变 化 的 量 。 
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设 动 态 加 速度 噪声 v, 为 独立 零 均 值 高 斯 加 速度 噪声 ,定义 为 


v, ^ N(0,0,) (18 - 6) 
这 里 符号 ~ Rav, KA wast MO, Q), HF 
q... Q. 0, 0, 
Q, = | 0, qy nQ 0, | (18 - 7) 
0, 0, GznQi 


对 于 连续 运动 对 象 ,Q, 由 文献 [3] 给 出 , 即 
TOT 
(18 - 8) 








din Vi 是 一 个 重要 的 滤波 器 设计 参数 。 当 研究 各 种 滤波 器 设计 选择 时 ,基于 
期 望 的 物体 运动 的 特点 (例如 ,对 象 是 机 动 还 是 直线 运动 ) ,给 出 如 何 选择 gj。 


18.1.2 传感器 数据 模型 


正如 18. 3.2 节 指 出 的 ,多 数 雷 达 传 感 器 的 输出 观测 (测量 ) 数据 是 二 维 极 
坐标 或 三 维 球 坐 标 ,而 与 动态 模型 所 选 的 坐标 系 没有 关系 。 因 此 ,将 完整 的 传 感 
器 数据 矢量 定义 为 


z, = [R,,R,,0,,$,]' (18 -9) 


其 中 集合 1R, ,R, ,9, ,4, | 分 别 为 待 跟踪 目标 相对 于 固定 传感器 位 置 的 距离 ,多 
普 勒 (距离 变化 率 ) ,方位 和 高 程 , 对 于 那些 没有 完整 观测 分 量 的 传感器 ,只 能 放 
弃 那 些 对 于 特定 雷达 无 法 获取 的 分 量 ,例如 ,在 后 面 用 于 性 能 分 析 的 仿真 中 ,所 
选 的 传感器 模型 中 只 包含 距离 方位 和 高 程 。 

通过 后 面 式 (18 - 160) 和 式 (18 - 161) 定义 的 非 线性 球 坐 标 到 笛 卡 儿 坐标 
变换 ,得 观测 矢量 与 笛 卡 儿 状 态 矢量 之 间 关 系 , 即 


R hu) rv, + 2 + p 9. 
: ! 
r = R, = h,(x,) ES 1, (x, ) = an (2) (18 e 10) 
0, h; ,(%,) r, 
中 h,,(x,) 





tan ` | 7 : 3 
通常 传感器 噪声 包含 在 观测 模型 中 ,因此 观测 模型 变 为 
z, = h,(x,) + w, (18 - 11) 
观测 噪声 w, 与 零 均 值 高 斯 噪声 无 关 , 定 义 为 
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w, ^ A(QO,R,) (18 - 12) 
R, 通常 认为 与 时 间 无 关 ,可 以 用 传感器 测量 值 方差 的 对 角 协 方差 矩阵 定义 为 
c, 0 0 0 
0 o 0 0 
0 0 co 0 
0 0 0 oc 


对 于 真实 传感器 数据 ,在 每 一 个 时 间 传 感 器 更 新 时 ,通常 作为 传感器 数据 流 
的 一 部 分 输出 传感器 测量 标准 偏差 .如 果 传 感 探 测 器 的 输出 信 噪 比 不 断 变化 ,所 
以 R 的 分 量 应 该 是 时 间 相 关 的 ,对 于 以 下 性 能 分 析 用 到 的 仿真 , 设 R 为 不 随时 间 
变化 的 。 


18. 1.3 高 斯 跟踪 算法 计算 笛 卡 儿 状 态 矢量 
18.1.3.1 向 卡 儿 线 性 状态 预测 方程 


因为 笛 卡 儿 动 态 方程 是 线性 的 ,可 以 使 用 表 6 - 1 中 的 线性 状态 方程 和 状态 
协 方 差 预测 方程 。 


(18 - 13) 


PPN = PF. Kua a (18 - 14) 
Piin-1 = F, PE ant Bani +O (18 - 15) 


18. 1.3.2 向 卡 儿 非 线性 观测 预测 


由 式 (18 - 10) 可 见 , 观 测 模型 关于 状态 矢量 是 非 线 性 的 ,所 以 必须 用 第 TI 
部 分 的 一 种 次 优 滤 波 器 进行 观测 预测 .在 这 个 实例 中 , 仅 使 用 EKF 和 除 MCKF 
之 外 所 有 的 Sigma 点 卡尔 曼 滤波 器 。 
18. 1.3.2.1 第 卡 儿 线性 化 EKF 观测 预测 方程 ,根据 表 7 - 2, EKF 观测 预测 方 
程 为 


Zn - b xu) (18 zm 16) 
Print = N... PZ ee + R, (18 = 17) 
Pa nt = p? BL. (18 = 18) 


其 中 式 (18 - 164) ~ 式 (18 - 190) 给 出 了 雅 可 比 矩 阵 的 元 素 Huni o 
18.1.3.2.2 第 卡 儿 非 线 性 Sigma 点 观测 预测 方程 ,根据 图 13 - 3, 给 出 Sigma 
点 卡尔 曼 滤波 器 预测 方程 为 
By = [FS Er (18 - 19) 
X41 = P * D, , c" (18 - 20) 
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yi = h (x) (18 «« 91) 


nl n-1 nin-l 
N 
^ 


Zain- = bx Wr, (18 - 22) 


ia 


N, 
Prin-t = EJ wur, = Zaiat) (Pini 7 Znin-1 p *R,, (18-23) 
j=l 
N 


Pin = Y wi dai 一 in ) (Pini 一 AR y (18 - 24) 
这 里 集合 | e | 可 以 用 列表 13 - 1 和 表 13 - 2 离线 计算 (参见 第 13 28) 。 
18.1.3.3 卡尔 曼 滤波 器 更 新 方程 


跟踪 估计 的 最 后 步骤 是 使 用 卡尔 曼 滤波 器 更 新 方程 (5 -29) -方程 (5 -31)， 
即 


K, = PPS i] (18 - 25) 
is = È ai + K, (z, a Esa) (18 sii 26) 
Pie = P | Ps, (18 - 27) 


18.2 球 坐 标 系 下 目标 跟踪 


本 节 将 讨论 利用 球 坐 标 系 下 状态 矢量 进行 跟踪 目标 ,状态 矢量 定义 为 
p = [R,v,0,v,,0,v,]' (18 - 28) 
其 中 球速 度 分 量 1v, ,vw ,v,| 将 在 后 面 介 绍 。 与 笛 卡 儿 坐 标 系 下 情况 类 似 , 假 定 笛 
卡 儿 速度 分 量 为 常数 ,导出 式 (18 -1) 的 笛 卡 儿 坐 标 系 动态 运动 模型 ,这 里 研究 
目标 是 建立 具有 恒定 球速 度 分 量 的 目标 的 三 维 动态 运动 模型 。 


18.2.1 球 坐标 系 下 状态 矢量 的 位 置 和 速度 分 量 
参考 图 18 - 25 ,目标 的 位 置 可 写 为 


r, = xe, + ye, + ze, (18 — 29) 
r, = a,€, + a,€, + a,€, (18 - 30) 
其 中 ,|e.,e,,e:| 和 | en ,eo,e。! 为 分 别 在 笛 卡 儿 坐 标 系 和 球 坐 标 系 下 的 单位 矢 
HERA. 
位 置 分 量 从 球 坐 标 系 到 笛 卡 儿 坐 标 系 的 变换 矩阵 由 式 (18 - 193) 定义 ,所 
以 可 以 将 式 (18 - 29) 改写 为 





r, = e,Rsingcos 由 + e,Rcosócos$ + e.Rsing (18 — 31) 
根据 定义 , 球 坐 标 系 单位 矢量 为 
l or 
g, m — (18 - 32) 
gm | R 
R 
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or 








(18 - 33) 


(18 — 34) 


将 式 (18 - 31) 代入 式 (18 - 32) ,根据 集合 |e, ,e,,e.| Hee, 的 表达 式 为 


e, = e sinĝcosh + e,cosgcos 中 + e.sing 
类 似 地 ,得 
€, = e,cos0 — e,sinó 
€, =- e,sinOsinj — e,cosQsinó + e:cos 中 
以 下 给 出 了 球面 单位 矢量 的 时 间 导 数 , 即 
0€,. ðe,- de. 


e, = oR t ag? * aa? 


= e,0cos$ + ev 中 


一 e cosp + e ,0sin$ 
9e, . ñ ae, . ri 
aR 00 0 中 
= 一 e,0sind - eg 
利用 式 (18 - 35) ,可 以 将 式 (18 - 31) 写成 
r, = e.Rsingcos 中 + e, Rceos0cosd + e,Rsind 
= R| e,sin0cosd + e,cosgcos 由 + esing | 
= Re. 
由 于 速度 矢量 是 位 置 矢 量 关 于 时 间 的 导数 ,得 


d : . 
‘=< a C R62 = Re, + Re, 


= e,R + e,R0cos$ + e,Ró 
确定 球 坐 标 系 中 的 分 量 v 为 


v, 
v= U, 
v, 








R R 
Récoso | = R,0 
R$ Ró 


(18 - 35) 


(18 — 36) 
(18 - 37) 


(18 — 38) 


(18 - 39) 


(18 - 40) 


(18 - 41) 


(18 - 42) 


(18 - 43) 


这 里 定义 水 平 距离 为 RA Reos 由 利用 式 (18 - 43) 作为 速度 分 量 , 式 (18 - 28) 


给 出 了 球 坐 标 系 状态 矢量 的 所 有 分 量 。 
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18.2.2 球 坐 标 系 状 态 矢 量 动 态 方程 


对 于 恒 速 度 目 标 运动 动态 模型 ,假设 v，= v, = v, = 0。 取 式 (18 - 43) 各 个 
分 量 关 于 时 间 的 导数 ,得 


v, =R=0 (18 - 44) 
v, = ROcosh + ROcosh - Rójsinó = 0 (18 - 45) 
v, = Rb + R$ = 0 (18 - 46) 
或 者 ,重新 整理 ,得 
5 = 6(dtang - $) (18 - 47) 
sk: _ 
$ = n? (18 - 48) 
下 面 将 给 出 动态 运动 方程 的 分 量 。 距 离 可 以 写成 
R. = R.., + TR,., + TR. (18 - 49) 
将 式 (18 - 44) 和 式 (18 — 43) rh v, 的 定义 代入 上 式 , 变 为 
R, = R, + Tv,, (18 - 50) 
对 于 径 向 速度 ,有 
9. = Vat T9, = o, (18 — 51) 


用 类 似 于 式 (18 - 49) 的 形式 写 出 方位 方程 ,用 式 (18 - 43) 速度 分 量 的 定 
义 取代 式 (18 - 47) 中 的 9, ,得 


0. = 6,, "Fori “5 (v, atnab a 70,42] (18 - 52) 
与 式 (18 - 51) 形式 类 似 , 有 


Van = Uhn-1 (18 - 53) 
此 外 ,高 程 分 量 为 

$ = boi eT Tu an (18 - 54) 
9,5 = Veni (18 - 55) 

球 坐 标 状态 矢量 的 非 线性 动态 运动 方程 可 以 写成 
Pr =F (Pri) + n, (18 - 56) 

其 中 零 均 值 高 斯 动态 加 速度 噪声 分 布 为 

n, ^ N(0,0,) (18 - 57) 


fip.) 的 元 素 由 非 线性 方程 (18 - 50) ~ 式 (18 - 55) 以 及 下 式 给 出 , 即 
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q,Q, 0, 0, 
Q, w | 0, qQ, 0, | 

0, 0, q,Q, 
T 
2 





T 


18.2.2.1 非 线 性 球 坐 标 动态 方程 的 远程 限制 


(18 - 58) 


(18 - 59) 


通过 使 用 式 (18 -43) ,分 别 利 用 90,_, 和 中 ,替换 式 (18 -52) 和 式 (18 - 54) 
中 的 v. n-1/ R4 nat fI v, , ,/R, 限定 R, 4 — 99 X8 - 50) = 式 ( 18 -55) 变 为 


线性 便 速 率 动 态 方程 , 即 


这 种 情况 下 t, 时 刻 的 球 坐 标 状态 矢量 可 以 表示 为 


Fa = [ R,,R,,0,,0, ,中 ,中 


动态 方程 变 为 线性 方程 , 即 


q,Q, 0, 0, 
Q, = 0, q Q, 0, 
0, 0, q, Q, 


18.2.3 球 坐 标 系 状态 矢量 观测 方程 


(18 - 60) 
(18 - 61) 
(18 - 62) 
(18 - 63) 
(18 - 64) 
(18 - 65) 


(18 - 66) 


(18 - 67) 


(18 - 68) 


(18 - 69) 


(18 - 70) 


式 (18 -9) 给 出 了 传感器 数据 矢量 .基于 这 个 数据 矢量 ,可 写 出 与 式 (18 756) 
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或 式 (18 - 67) 球 坐标 动力 学 方程 相关 的 线性 观测 方程 , 即 


z, = Hp, +W, (18 - 71) 

= Hr, *w,, (18 - 72) 
I 0 0 0 0 0 

H- 0 10 0 0 0 (18 - 73) 
00 10 0 0 
00001 0 


18.2.4 高 斯 跟踪 算法 计算 球 坐 标 状 态 矢 量 


18.2.4.1 当 R 很 大 时 的 跟踪 估计 


HES EA FPR > 1! 的 情况 ,可 以 看 到 ,动态 方程 和 观测 方程 即 式 (18 -67) 
和 式 (18 - 72) 都 是 线性 的 ,因此 第 6 章 提出 的 线性 卡尔 曼 滤波 器 是 一 个 最 优 滤 
波 器 。 第 6 章 给 出 这 种 情况 的 LKF 状态 预测 方程 为 


Pana = FP | (18 = 74) 
Pica = FP uF + Q, (18 - 75) 
F fll Q 分 别 由 式 (18 - 68) 和 式 (18 — 70) 给 出 。 
观测 预测 方程 可 以 写成 
Za = Bf... (18 - 76) 
P= ,= HPZ _H +R (18 - 77) 
Pra = Pint (18 - 78) 
H f R 分 别 由 式 (18 — 73) 和 式 (18 - 13) 给 出 。 
卡尔 曼 滤波 器 更 新 方程 可 写成 
K, = Pina [P#, l” (18 - 79) 
Pun = Pana + Ez, — Zin) (18 - 80) 
P. = Pani - K,P=,_K,, (18 - 81) 


18.2.4.2 非 线 性 球 坐标 动态 模型 的 跟踪 估计 


球 坐 标 系 状态 矢量 的 实例 与 第 卡 儿 坐 标 状态 矢 量 的 实例 正好 相反 。 对 于 以 
恒定 币 卡 儿 速 度 运动 的 笛 卡 儿 状 态 矢量 ,其 状态 预测 方程 是 线性 的 ,但 观测 方程 
是 非 线 性 的 ,需要 用 次 优 非 线性 跟踪 滤波 器 进行 观测 值 预 测 , 现 在 ,状态 矢量 预 
测 方程 是 非 线性 的 ,而 观测 预测 方程 是 线性 的 ,需要 一 个 次 优 非 线性 跟踪 滤波 器 
进行 状态 预测 ,下 面 ,首先 研究 在 状态 预测 步骤 使 用 线性 卡尔 曼 滤波 器 所 需要 进 
行 的 改进 ,然后 考虑 Sigma 点 方法 。 
18.2.4.2.1 在 球 坐 标 系 中 应 用 EKF, AEH EKF 在 球面 坐标 系 中 进行 状态 预 
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iW ,必须 首先 找到 非 线 性 状态 转移 方程 的 雅 可 比 矩 阵 , 即 
p, = f(p,-.) (18 - 82) 
其 中 式 (18 - 50) ~ 式 (18 - 55) 中 给 出 转移 方程 的 元 素 . 这 样 EKF 状态 预测 方 
程 为 
Pasa =A Pita) (18 — 83) 
Pr = P® FT + 0, (18 - 84) 


HF, 是 转移 方程 (18 - 82) fE c, ,时刻 的 雅 可 比 矩 阵 。 


采用 附录 18. B 的 一 般 方法 , 式 (18 - 82) 的 雅 可 比 矩 阵 可 由 下 式 给 出 , 即 
l T 00 0 O 








0 1 0 0 0 0 
& | F. d F. Fu F 
F, = 31 32 34 35 36 (18 - 85) 
0 00 1 0 0 
Fs, F's, 0 0 1 F, 
0. 0:8 0.0 = 1 
T 
Fy =- pall + Cntanó - s.) | (18 - 86) 
T°u, 
T T 
E RL! + (tan -v,)] (18 — 88) 
T 
F. = spp see $ (18 - 89) 
h 
Tv, 
Fy, = 2RR, "Ó (18 - 90) 
Tv, Tv, 
Fy =- Z|! + ul (18 — 91) 
Tv 
F, = — 18 — 92 
52 2R? ( ) 
Tr. A 
Fs =pl +E (18 - 93) 


这 里 为 清晰 起 见 , 移 除 了 下 标 no 
因为 观测 矢量 中 没有 与 v RE v, 相关 的 元 素 , 观 测 预测 方程 和 更 新 方程 与 
18. 2. 4. 1 节 给 出 的 球 坐标 LKF 的 方程 相同 。 
18. 2.4.2.2 在 球 坐 标 系 中 应 用 Sigma 点 卡尔 曼 滤 波 器 -流程 的 第 一 步 是 根据 
前 面 滤波 器 的 输出 jp Pu. d 生 成 一 组 Sigma 点 ,或 者 由 初始 值 利 用 下 式 
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ies 5 yaixa * D, i, 1 62 j = 0,1,=<,N, (18 - 94) 
Donat = (Pen, a1" (18 - 95) 
用 列表 13 -1 和 13 - 2 中 给 出 的 子 程序 来 生成 集合 jj,e”,J 20,1, LE 
们 将 取决 于 特定 的 Sigma 点 卡尔 昌 洲 波 器 的 选择 和 状态 矢量 的 维 数 
下 面 给 出 Sigma 点 卡尔 曼 滤波 器 状态 预测 方程 为 
Ij, KE, P. (18 - 96) 


" (= Nuno. (18 - 97) 


PT, = > u (TI. - Pan) (MY), ST (18 - 98) 


* Q, (18 - 99) 
与 球 坐 标 系 EKF 一 样 , 线 性 观测 预测 方程 和 更 新 方程 与 上 面 给 出 的 球 坐 标 LKF 
的 方程 相同 。 


18.3 第 卡 儿 坐标 系 和 球 坐 标 系 下 跟踪 滤波 器 的 实现 


为 了 实现 前 面 提 出 的 跟踪 滤波 器 ,必须 要 

(1) 指定 一 组 9 值 ; 

(2) 生成 含 噪 数据 的 蒙特 卡 洛 集合 作为 滤波 器 的 观测 值 ; 
(3) 生成 初始 化 状态 矢量 和 协 方差 矩阵 。 

以 下 各 节 逐 一 解决 这 几 个 问题 。 


18.3.1 设置 9 值 


根据 式 (18 - 15) 的 笛 卡 儿 状 态 协 方差 矩阵 的 预测 方程 ,如 果 将 式 (18 - 15) 
等 式 左边 元 素 的 左上 角 2 x 2 集合 与 类 似 的 由 式 (18 - 8) 得 的 元 素 集合 Q 进行 
比较 ,得 








2 Pj a. 

PE: c ° r 3 2 
» aA? | 。 zi] p (18 - 100) 

Ë T q> q. T 


因此 o? < gq.7,g, BO Gre [ P708 ] 。 或 者 ,可 以 写成 [ao xt] 或 者 [a*/f]。 
假设 已 知 试图 跟踪 目标 的 某 些 机 动 特征 ,可 以 估算 出 所 需 q, 值 。 因 为 动态 
模型 是 零 均 值 高 斯 加 速度 噪声 的 恒 速 度 模型 ,可 以 设置 为 目标 最 大 可 能 机 动 加 
速度 的 2 倍 。 它 增加 了 一 倍 来 表示 向 左 或 向 右 转 .所 以 设 
q, = (2a)° (18 - 101) 
如 果 用 g&g( 重 力 加 速度 ) 来 表示 a, 必 须 将 单位 变换 为 Cn mile/s? )? ofin, im 
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果 一 个 目标 最 大 转弯 加 速度 为 3g, 则 有 
q, = (2 x 3g)! x (32. 174ft/s2/g)2 x (1.65 x 10 n mile/ft)? 
= 35 x 2. 80 x 105 (n mile/s’ )* (18 - 102) 
由 式 (18 -7) 可 知 ,g 的 各 个 分 量 可 以 设置 为 不 同 值 ,因此 ,对 于 一 个 笛 卡 儿 
滤波 器 ,可 以 将 水 平 机 动 值 (g, 和 9,) 设 定 为 一 个 值 ,而 将 垂直 机 动 (q.) 设 定 为 
男 一 个 值 , 对 于 球 坐 标 滤波 器 ,用 集合 1g, dng, | 表示 恒 球速 度 滤波 器 ,用 |g,， 
qasd! 表示 恒 球 速率 滤波 器 。 一 般 来 说 ,可 以 设 % 和 g 与 币 卡 儿 水 平 g 值 相等 ， 
设置 g, 的 值 等 于 笛 卡 儿 垂 直 %.。 对 于 恒 球 速率 滤波 器 ,很 容易 看 出 9 的 角度 可 以 
BH q, = q,/R, fll q, = 9,/R ,后 面 将 看 到 , 球 坐 标 下 q 值 越 高 ,性 能 越 好 。 


18.3.2 雷达 观测 数据 仿真 


为 对 跟踪 估计 算法 进行 性 能 比较 ,对 每 种 跟踪 算法 必须 在 给 定 的 蒙特 卡 洛 
次 数 上 用 相同 的 含 品 观测 值 . 对 于 每 次 蒙特 卡 洛 运行 ,必须 用 一 组 新 的 .与 前 面 
设置 无 关 的 含 品 数据 生成 一 组 不 同 的 观测 值 . 所 以 ,如 果 要 使 用 100 次 蒙特 卡 洛 
运行 来 比较 多 目标 跟踪 估计 算法 的 性 能 , 则 需要 100 组 含 噪 观测 值 ,噪声 与 时 间 
采样 无 关 , 也 和 蒙特 卡 洛 集合 无 关 。 
跟踪 目标 并 生成 航 迹 , 利 用 附录 18. A 给 出 的 低 精 度 仿 真 。 忽 略 大 气 往返 传 
播 对 雷达 信号 以 及 雷达 探测 器 检测 统计 数据 的 影响 ,假定 探测 概率 为 100% “E 
成 观测 集 的 特征 ,雷达 必须 设置 ; 
(1) 雷达 观测 矢量 由 哪些 变量 构成 ; 
(2) 什么 是 雷达 更 新 率 ; 
(3) 每 个 观测 变量 的 观测 噪声 标准 偏差 是 多 少 。 
对 于 一 个 普通 雷达 ,其 观测 矢量 可 以 由 目标 的 距离 (R) 和 距离 变化 的 速率 
CR) 构成 ,目标 相对 于 正 北向 的 水 平方 位 (9) 和 目标 在 地 面 以 上 的 高 度 ($8) ,这 
FE ,观测 矢量 可 写成 
z = [R,R,0,ġ]" (18 - 103) 
注意 :在 球 极 坐标 系 中 - 180° < 0 < 180° H 0? < $ = 90°, 
通常 假定 观测 矢量 的 每 个 元 素 的 噪声 为 高 斯 分 布 且 无 关 , 因 此 得 观测 噪声 
协 方差 窍 阵 为 
ao, 0 0 0 
tal 9 “P (18 - 104) 
0 0 æ 0 
0 00 c 
式 中 :co 为 第 ;个 观测 矢量 元 素 噪声 的 标准 偏差 。 
在 下 一 节 给 出 的 跟踪 估计 性 能 结果 中 ,采用 一 个 只 有 距离 ,方位 和 高 程 的 雷 
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达 模 型 用 于 生成 含 品 观 测 值 的 观测 不 确定 性 为 


Tr = 0. 15n mile (18 - 105) 
T, = 0.3° (18 — 106) 
o, = 0.3° (18 - 107) 


仿真 雷达 的 更 新 周期 为 每 4s 一 个 观测 值 , 更 新 频率 为 0. 25Hz。 

利用 附录 18. A 中 给 出 的 方法 ,可 生成 笛 卡 儿 目标 航 迹 ,以 10Hz 的 频率 生成 
仿真 航 迹 数 据 。 因 为 雷达 模型 的 更 新 频率 低 得 多 , 航 迹 数据 必须 首先 降低 采样 至 
雷达 更 新 频率 ,以 得 添加 观测 噪声 之 前 的 观测 值 的 真实 值 ,当然 也 可 以 增加 一 个 
开始 时 间 变 量 ,但 对 于 这 里 的 目标 来 说 是 没有 必要 的 ,所 以 选择 在 t。= 0 时 开始 
观测 。 如 果 使 用 一 个 可 变 的 开始 时 间 , 而 且 开 始 时 间 包 含 多 于 一 个 小 数位 (例如 
to = 0. 12345s) ,必须 使 用 插值 来 根据 航 迹 数据 生成 笛 卡 儿 采 样 值 . 如 果 原 始 航 
迹 包括 4000 个 采样 点 ,最 终 会 得 100 个 采样 点 来 生成 观测 值 集合 。 

假设 降 采 样 后 的 球 坐 标 真 实数 据 包 含 在 可 变 极 坐标 真实 数据 ( 维 数 为 P x 
N, Hp P 是 观测 矢量 的 维 数 ,而 入 是 时 间 采 样 点 的 数目 ) 中 ,用 列表 18 -1 的 
Matlab 程序 片段 将 噪声 添加 到 真实 航 迹 采样 值 中 ( Stream 是 一 个 标志 符 ,为 最 新 
版 本 的 randn 函数 中 的 随机 数 流 ) 。 
列表 18 - 1 加 入 随机 观测 噪声 

Observations = PolarTruth + sqrtm(R) *randn(Stream,P,N); 

为 每 次 蒙特 卡 洛 运行 生成 一 组 独立 的 观测 值 。 每 次 蒙特 卡 罗 运 行 最 后 的 观 
测 值 集 合 包含 N 个 样本 。 这 一 过 程 可 以 离线 完成 , 且 与 跟踪 性 能 计算 无 关 。 


18.3.3 滤波 器 初始 化 


所 有 序 贯 卡尔 曼 跟 踪 滤 波 器 都 需要 计算 状态 矢量 和 协 方差 矩阵 , 分 别 为 


Xoo 和 PR 。 假 设 要 访问 传感器 数据 流 ( 如 上 所 述 ,可 以 是 仿真 的 ,也 可 以 是 真实 
的 ) ,最 简单 的 方法 是 利用 开始 的 少数 传感器 观测 数据 ,来 创建 竺 卡 儿 状态 矢量 
的 初始 估计 ,进而 生成 所 需 的 初始 值 。 

如 果 用 一 个 完整 的 含 噪 球 极 坐标 6 维 状态 矢量 作为 观测 矢量 ,可 以 通过 简 
单 的 对 含 品 球 坐标 状态 矢量 数据 的 一 些 样本 取 平 均 , 然 后 用 式 (18 - 195) 定义 
的 极 坐标 — 笛 卡 儿 坐 标 变换 ,来 生成 笛 卡 儿 状态 矢量 的 初始 值 . 但 是 在 数据 矢 
量 中 ,最 多 只 能 有 6 个 球 坐 标 分 量 中 的 4 个 。 

由 于 不 可 能 从 观测 值 中 得 球 坐 标 状态 矢量 的 所 有 分 量 ,对 那些 无 法 直接 由 
传感器 数据 中 获取 的 球 坐 标 分 量 , 需 要 设置 默认 值 . 最 简单 的 实现 方式 是 为 球 坐 
标 状态 矢量 和 它们 对 应 的 标准 偏差 的 所 有 元 素 生成 一 组 额定 值 , 一 般 来 说 ,所 有 
的 传感器 都 提供 方位 及 方位 的 不 确定 性 信息 ,假定 总 是 可 以 从 传感器 获得 这 些 
数据 ,不 用 设置 额定 值 ,已 经 创建 一 组 额定 的 球 坐 标 初始 值 ,用 在 所 有 的 目标 跟 
踪 滤 波 器 当中 进行 性 能 分 析 , 这 些 都 列 在 表 18 - 1 中 。 在 球 坐标 初始 协 方差 矩阵 
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中 ,将 所 有 非 对 角 项 设置 为 零 。 
表 18 -1 初始 状态 矢量 和 协 方差 元 素 的 额定 值 

距离 /n mile 10 L 

距离 变化 率 /kn - 600 

方位 /rad 可 由 传感器 获取 可 由 传感器 获取 

方位 变化 率 /( rad/s) 107 

高 程 /rad 10? 

高 程 变化 率 /( rad/s) io* 

当 得 一 个 或 多 个 球 坐 标 元 素 观测 数据 时 ,可 以 根据 连续 的 样本 数据 计算 初 

始 值 ,根据 位 置 数据 (R,90,4$) ,通过 对 连续 数据 值 取 平 均 来 生成 一 个 初始 值 , 例 
如 可 以 对 前 两 个 数据 样本 取 平 均 ,得 距离 的 初 值 , 即 


R, = Z (R, + R.) (18 - 108) 


对 于 速率 元 素 , 可 以 用 前 3 个 数据 点 的 2 个 ,由 式 (2 744) 来 计算 中 心 差 分 公式 ， 
例如 可 以 计算 距离 变化 率 为 





R, = F(R, - R.) (18 - 109) 


当 处 理 异 步 多 传感器 数据 时 ,必须 进行 一 些 改进 ,因为 某 些 分 量 可 能 从 一 种 传 感 
器 中 获取 ,而 在 另 一 种 传感器 中 得 不 到 。 

如 果 得 了 完整 的 球 坐 标 状态 矢量 和 协 方差 矩阵 的 初始 值 , 就 可 以 用 附录 
18. B 给 出 的 公式 进行 简单 的 球 坐 标 一 笛 卡 儿 坐 标 变换 , 求 得 笛 卡 儿 状态 矢量 和 
协 方差 矩阵 的 初始 值 。 要 确保 笛 卡 儿 坐 标 系 下 初始 状态 矢量 和 协 方差 矩阵 集合 
是 给 定 的 ,并 且 对 于 所 有 蒙特 卡 洛 运行 和 所 有 跟踪 估计 方法 都 是 相同 的 。 

NET RR Al p fl i SUE UR ,这些 笛 卡 儿 的 初始 值 必须 变换 回 球 坐 标 , 生 成 集合 


| rgo Pool ,并 将 其 转换 为 初始 化 集合 为 [Pmo Peo! 的 恒 速度 球 坐 标 滤 波 器 。 注 
意 , 由 r 到 p 的 变换 由 下 式 给 出 , 即 


R R 
Vp R 
6 
p = 0 = g(r) = . (18 - 110) 
Vh 0Rcoso 
中 
U $R 
pe = GPrG' (18 - 111) 
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式 中 :6G 为 变换 的 雅 可 比 矩 阵 8(r) 计算 雅 可 比 矩 阵 的 偏 导 , 得 


l 0 0 0 0 0 

0 1 0 0 0 0 

^ 0 0 1 0 0 0 
G 3|. (18 - 112) 

gcos 0 0 hRcosp -Rosin®g 0 

0 0 0 0 l 0 

Ó 0 0 0 0 R 


18.4 各 种 估计 方法 的 性 能 对 比 


18.4.1 用 于 性 能 分 析 的 轨迹 特征 


为 便于 比较 分 析 , 用 附录 18. A 列 出 的 方法 生成 3 个 目标 运动 场景 ,图 18 -1 
至 图 18 - 3 分 别 表示 与 3 个 场景 其 中 一 个 相关 的 航 迹 。 

第 一 个 场景 航 迹 如 图 18 - 1 所 示 ,是 以 恒定 笛 卡 儿 速 度 径 向 运动 的 目标 。 这 
个 航 迹 中 ,目标 以 1n mile 的 恒定 径 向 人 站 高 度 ( 在 图 18 - 1 中 没有 显示 高 度 ) 运 
动 , 并 直接 通过 位 于 (0,0,0) 的 雷达 传感器 。 第 二 个 场景 航 迹 如 图 18 - 2 所 示 ， 
是 进行 两 次 转弯 的 目标 ,第 一 次 是 90°2g 的 转弯 ,第 二 次 是 90°5g 的 转弯 ,第 三 个 
航 迹 是 一 个 高 度 机 动 目标 ,如 图 18 - 3 所 示 , 目 标 首先 经 历 一 个 7g 的 转弯 后 , 紧 
接着 一 个 Og 的 转弯 ,减速 后 进行 5g 的 俯冲 ,平稳 下 来 后 又 加 速 , 最 后 是 7g 的 转 
弯 , 所 有 这 些 场景 都 类 似 于 Zollo 和 Ristic ^ 所 提出 的 场景 。 
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图 18 -1 仿真 径 向 人 站 目标 真实 轨迹 


表 18 -2 给 出 了 3 个 场景 的 具体 初始 特征 值 . 对 于 用 于 性 能 分 析 的 所 有 跟踪 
滤波 器 ,将 9 的 分 量 值 设置 为 : 笛 卡 儿 跟 踪 器 |q。= q, = 225;q, = 1.5} , 球 坐 标 
恒 速 度 跟踪 器 |4，= q, = 225;q, = 1.5} , 球 坐 标 恒 速 率 跟踪 器 jg,，= 225;q; = 
qs =1.5} sq (ARR 225 还 是 1.5 ,取决 于 预期 的 最 大 转弯 / 俯冲 加 速度 为 7. 5g 还 
fé 0. 61g. 
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图 18 -3 仿真 基准 测试 目标 真实 轨迹 
表 18 -2 3 个 仿真 场景 的 初始 特征 值 


首先 检查 每 个 场景 中 速度 特征 的 航 迹 ,来 演示 如 何 使 航 迹 符合 任 一 种 恒 速 
度 模型 .图 18 -4 至 图 18 -6 给 出 了 3 个 场景 在 笛 卡 儿 坐 标 系 和 球 坐 标 系 下 的 速 
度 分 量 。 根 据 图 18 - 4, 径 向 人 站 目标 的 笛 卡 儿 速 度 很 显然 是 恒定 的 ,而 当 轨 迹 
在 原点 穿 过 传感器 的 区 域 时 ,距离 和 垂直 速度 的 球 坐 标 速度 分 量 都 有 突然 变化 
的 迹象 ,因此 ,可 以 预测 :远离 原点 区 域 时 两 个 跟踪 器 模型 之 间 的 性 能 差异 很 小 ， 
但 是 当 目 标 接近 原点 时 球 坐 标 跟 踪 占 会 遇 到 一 定 的 困难 。 虽 然 在 图 中 没有 显示 ， 
但 是 , 当 目 标 经 过 原点 时 ,由 于 突然 变化 , 恒 速 率 球 坐 标 滤波 器 中 的 速率 在 很 短 
的 距离 中 显示 出 较 大 的 变化 。 

比较 图 18 - 5 中 环形 航 迹 的 场景 速度 ,可 见 速 度 在 笛 卡 儿 坐 标 和 球 坐 标 中 
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图 18 -5 环形 航 迹 的 笛 卡 儿 速 度 和 球 坐 标 速度 分 量 


显示 相似 的 变化 ,所 以 跟踪 性 能 的 差异 应 该 比较 小 ,对 于 如 图 18 - 6 所 示 高 度 机 
动 航 迹 ,由 于 作为 时 间 的 函数 ,其 速度 上 的 变化 相似 ,图 中 速度 分 量 表 明 球 坐标 
跟踪 如 的 运行 效果 应 该 和 笛 卡 儿 跟 踪 器 相似 。 
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图 18 -6 基准 测试 航 迹 的 笛 卡 儿 速 度 和 球 坐 标 速度 分 量 
18.4.2 滤波 器 性 能 对 比 


在 笛 卡 儿 坐 标 和 球 坐 标 跟 踪 器 中 ,采用 相同 的 跟踪 估计 滤波 器 (EKF UKF、 
SSKF 和 CHKF ) ,为 方便 区 分 ,分 别 用 前 级 C — 和 S - 来 区 分 笛 卡 儿 坐 标 和 球 坐 标 
跟踪 算法 ,用 4 个 笛 卡 儿 坐 标 卡尔 曼 滤波 器 C - EKF,C - UKF,C -SSKF 和 C - 
GHKF 来 生成 笛 卡 儿 恒 速度 模型 的 跟踪 轨迹 ,再 用 4 个 球 坐 标 卡尔 曼 滤波 器 S - 
EKF,S - UKF S -SSKF 和 S - GHKF 来 生成 球 坐 标 恒 速 度 模型 的 跟踪 轨迹 ,对 
于 球 坐 标 恒 速 率 远 距离 估计 模型 ,采用 S - LKF 滤波 器 。 

对 每 种 情况 ,进行 100 次 的 蒙特 卡 洛 运 行 。 对 于 笛 卡 儿 跟 踪 算 法 ,将 笛 卡 儿 
坐标 估计 轨迹 输出 变换 为 球 坐 标 ; 而 对 于 球 坐 标 跟 踪 器 ,将 球 坐 标 跟 踪 输 出 变换 
为 笛 卡 儿 坐 标 , 所 有 的 坐标 变换 都 利用 附录 18. B 中 的 算法 进行 计算 ,因此 ,对 所 
有 的 跟踪 器 ,生成 笛 卡 儿 坐 标 和 球 坐 标 跟踪 估计 。 根 据 100 次 蒙特 卡 洛 运行 结 
果 , 在 笛 卡 儿 坐 标 系 和 球 坐 标 系 中 计算 均 方 根 误差 的 位 置 估计 。 


18.4.2.1 径 向 入 站 跟踪 航 迹 的 跟踪 性 能 比较 


图 18 -7 和 图 18 -8 给 出 分 别 用 笛 卡 儿 和 球 坐 标 系 卡 尔 曼 滤波 器 , 径 向 人 站 
目标 第 一 次 蒙特 卡 洛 运行 的 三 维 估计 轨迹 和 真实 轨迹 ( 灰色 线条 ) 。 由 这 些 估计 
跟踪 图 可 以 看 出 : 当 目 标 经 过 位 于 (0,0,0) 的 传 感 涡 时 , 球 坐 标 跟踪 器 效果 比 笛 
卡 儿 跟 踊 器 更 好 。 除 了 C - EKF ,其 它 笛 卡 儿 滤波 器 在 原点 附近 区 域 表 现 不 佳 。 

可 以 在 笛 卡 儿 跟 踪 估 计 方 法 的 非 线性 观测 方程 (18 - 10) 中 找 出 笛 卡 儿 跟 
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踪 估 计 在 原点 附近 发 散 现象 出 现 的 原因 .尽管 具体 细节 超出 了 本 书 的 范畴 ,结果 
表明 , 式 (18 - 10) 中 包括 的 反正 切 运算 造成 了 发 散 。 由 于 笛 卡 儿 位 置 分 量 的 大 
小 接近 于 估计 位 置 不 确定 性 的 大 小 ,任意 两 个 位 置 分 量 比值 的 反正 切 概 率 密 度 
函数 近似 于 均匀 分 布 。 因 此 方位 和 高 程 的 预测 都 由 越 来 越 大 的 标准 偏差 控制 , 导 
致 式 (18 - 26) 中 的 新 息 矢 量 (z。- Zuni) 变 得 不 稳定 ,进而 导致 了 滤波 器 发 散 。 然 
而 ,和 预期 一 样 ,在 远离 原点 的 区 域 , 笛 卡 儿 滤波 器 比 球 坐 标 系 滤波 需 效 果 更 佳 。 
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图 18 -8 用 球 坐 标 跟踪 方法 计算 径 向 人 站 目标 航 迹 的 估计 轨迹 


对 于 径 向 人 站 目标 航 迹 ,检查 图 18 - 9 中 笛 卡 儿 坐 标 系 和 球 坐 标 系 跟踪 算 
法 的 RMS 第 卡 儿 位 置 误差 对 比 ,以 及 图 18 - 10 中 两 类 跟踪 算法 的 RMS ERER 
系 位 置 误 差 对 比 。 在 这 些 图 中 , 设 q. = q, = q, = 4, = 225 , 且 所 有 其 它 4 值 均 为 
1.5。 很 明显 ,在 估算 笛 卡 儿 轨 迹 时 ,所 有 的 跟踪 器 运行 得 都 很 好 ,除了 在 大 约 
410s 目标 穿 过 雷达 区 域 时 。 在 这 一 点 之 后 , 球 坐 标 系 跟踪 器 表现 出 更 好 的 性 能 ， 
其 中 S - GHKF 是 最 好 的 。 在 近 距 离 跟踪 区 域 , 球 坐标 跟踪 器 在 高 程 上 表现 出 很 
大 的 RMS 误差 ,但 对 所 有 其 它 笛 卡 儿 和 球 坐 标 变量 给 出 了 额定 的 RMS 误差 。 
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图 18 -9 用 笛 卡 儿 坐 标 系 和 球 坐 标 系 跟踪 算法 计算 
径 问 人 站 目标 航 迹 的 RMS 稍 卡 儿 坐 标 位 置 误差 
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图 18 -10 用 笛 卡 儿 坐 标 系 和 球 坐标 系 跟踪 算法 计算 
径 向 人 站 目标 航 迹 的 RMS 球 坐 标 位 置 误差 


现在 检验 径 向 和 人 站 航 迹 中 g 值 变化 对 RMS 位 置 误差 性 能 的 影响 ,要 注意 ,在 
图 18 -9 和 图 18 -10 中 ,第 卡 儿 滤波 器 族 与 球 坐标 滤波 器 之 间 的 性 能 差别 很 小 。 
为 更 加 简单 地 检验 不 同 q 值 对 性 能 的 影响 ,这 里 仅 比较 当 g 变化 时 , 笛 卡 儿 和 球 
坐标 系 EKF 算法 的 性 能 结果 .尽管 本 书 这 里 没有 给 出 ,但 根据 对 4. angular FQ, (EL 
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变化 的 初步 研究 结果 ,把 它们 设 成 以 下 三 个 值 当中 的 一 个 :1.5.36 和 225。 从 分 
析 中 很 显然 可 以 看 出 ,对 于 所 有 航 迹 最 好 的 选择 是 1.5. 

由 于 径 向 人 站 航 迹 中 包含 在 整个 轨道 保持 恒定 速度 的 目标 ,有 读者 会 认为 ， 
RMS 位 置 误 差 将 会 与 4 的 值 成 正比 , 即 当 9 增加 时 ,RMS 误差 随 之 增加 。 然 而 , 仔 
细 观 察 图 18 - 11 和 图 18 - 12, 可 看 到 : 当 g 水 平 值 为 36 时 , 笛 卡 儿 跟 踪 估 计 性 
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图 18 - 11 笛 卡 儿 和 球 坐 标 系 EKF 滤波 器 中 不 同 4 值 下 ， 
径 向 人 站 航 迹 RMS 笛 卡 儿 位 置 误差 的 比较 
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图 18 -12 笛 卡 儿 和 球 坐 标 EKF 滤波 器 中 不 同 4 值 下 ， 
径 向 入 站 航 迹 RMS 球 坐 标 位 置 误差 的 比较 


236 


能 最 佳 ;而 对 于 球 坐 标 跟 踪 器 来 说 最 好 的 选择 是 550。 注 意 ,对 于 笛 卡 儿 滤 波 器 
算法 ,gq, 和 9。 分别 表示 水 平和 垂直 的 4 值 。 由 图 可 见 ,所 有 的 滤波 器 在 原点 附近 
高 程 估计 的 RMS 误差 都 很 大 , 球 坐 标 系 滤波 器 的 性 能 略 好 ,比较 结果 为 , 当 g，= 
550 时 的 S - EKF 的 性 能 最 好 。 


18.4.2.2 环形 目标 航 迹 跟踪 性 能 比较 


在 环形 航 迹 场 景 中 ,图 18 - 13 和 图 18 - 14 分 别 给 出 了 笛 卡 儿 跟 踪 算 法 和 球 
坐标 跟踪 算法 中 , 沿 真 实 轨迹 输出 跟踪 轨迹 的 例子 。 对 图 例 进行 观察 ,可 见 所 有 
的 追踪 器 除了 在 高 g 转弯 的 地 方 稍 有 延迟 ,其 余 都 表现 很 好 , 笛 卡 儿 跟 踪 器 在 远 
离 转弯 区 域 时 的 波动 稍 小 。 
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图 18 -13 向 卡 儿 跟 踪 方 法 在 环形 目标 航 迹 估计 中 的 跟踪 结果 


rev。 





X fr ' /n mile 
18 - 14 所 有 球 坐 标 跟踪 方法 在 环形 目标 航 迹 中 的 估计 轨迹 


RMS 位 置 误差 如 图 18 — 15 和 图 18 - 16 所 示 , 表 明 所 有 的 跟踪 估计 算法 具 
有 相似 的 性 能 , 笛 卡 儿 跟踪 算法 的 整体 性 能 稍 好 ,在 笛 卡 儿 跟 踪 器 中 ,C - SSKF 
是 性 能 最 差 的 跟踪 估计 算法 ;而 在 球 坐 标 跟踪 器 中 ,S - LKF 是 性 能 最 好 的 ,在 这 
两 个 图 中 , 设 4. = 9, = q, = q, = 225 ,并 将 所 有 q 值 都 设 为 1. 5。 
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图 18 -15 第 卡 儿 跟踪 算法 在 环形 目标 航 迹 中 的 RMS 第 卡 儿 和 球 坐 标 位 置 误差 
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图 18 -16 球 坐 标 跟踪 算法 在 环形 目标 航 迹 中 的 RMS 笛 卡 儿 和 球 坐 标 位 置 误差 


为 检验 不 同 q 值 的 效果 ,只 采用 笛 卡 儿 和 球 坐 标 EKF 来 考查 性 能 ,其 中 9.、 
angular 和 9, 值 设 为 1.5。 图 18 - 17 和 图 18 -18 分 别 给 出 了 RMS 笛 卡 儿 和 球 坐 标 
位 置 误差 的 结果 。 结 果 表 明 , 对 于 笛 卡 儿 和 球 坐 标 EKF , 当 水 平 4 值 分 别 设 为 225 
fll 550 时 ,跟踪 结果 最 好 。 
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图 18 -17 在 环形 航 迹 中 ,作为 g 的 函数 ,RMS 笛 卡 儿 位 置 误 差 性 能 比较 
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图 18 -18 在 环形 航 迹 中 ,作为 9 的 函数 ,RMS 球 坐 标 位 置 误差 性 能 比较 


18.4.2.3 ”高度 机 动 目标 航 迹 基准 测试 的 跟踪 性 能 比较 


图 18 - 19 和 图 18 -20 分 别 为 采用 笛 卡 儿 和 球 坐 标 跟踪 算法 ,对 最 后 一 个 航 
迹 的 跟踪 估计 ,其 中 有 一 些 高 g 机 动 飞行 ,还 有 减速 和 加 速 部 分 ,通过 比较 两 图 
可 见 , 币 卡 儿 系 和 球 坐 标 系 的 所 有 的 跟踪 算法 ,除了 在 图 表 末 端的 加 速 部 分 ,其 
余 效 果 都 很 好 ,第 卡 儿 跟踪 器 在 加 速 之 后 最 后 一 个 转弯 处 越过 了 目标 ;而 球 坐 标 
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跟踪 器 没 有 这 个 最 后 转弯 的 问题 从 这 些 图 中 ,很 难 确定 跟踪 性 能 最 好 的 跟踪 
算法 。 
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图 18 -19 所 有 笛 卡 儿 跟 踪 方 法 在 基准 目标 航 迹 测试 中 的 估计 轨迹 
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图 18 -20 所 有 球 坐 标 跟踪 方法 在 基准 目标 航 迹 测试 中 的 估计 轨迹 


图 18 -21 和 图 18 - 22 的 RMS 位置 误 差分 别 对 应 笛 卡 儿 和 球 坐 标 RMS 位 
置 误差 ,比较 可 知 , 对 于 高 度 机 动 目标 ,与 第 卡 儿 坐标 系 跟踪 算法 相 比 , 球 坐 标 系 
跟踪 算法 的 RMS 第 卡 儿 和 距离 位 置 误差 稍 低 , 此 外 , 球 坐 标 系 跟踪 估计 算法 的 
方位 跟踪 性 能 比较 好 ,但 在 高 程 跟踪 上 性 能 最 差 , 综 上 所 述 , 在 机 动 目 标 跟踪 过 
程 中 , 球 坐 标 跟踪 滤波 器 算法 得 的 跟踪 估计 与 笛 卡 儿 跟踪 估计 算法 效果 相似 , 稍 
优 于 第 卡 儿 算法 。 还 应 指出 的 是 ,在 所 有 滤波 器 中 ,恒定 球速 率 滤波 器 的 性 能 最 
好 。 对 这 种 目标 距离 不 小 于 20n mile 的 远 距离 跟踪 场景 ,这 并 不 奇怪 ,在 这 两 个 
Al Bq, = q, = q, = q, = 225 ,而 其 它 的 g 值 设 为 1.5。 

为 研究 不 同 9 值 对 航 迹 的 影响 , 只 检验 笛 卡 儿 和 球 坐 标 EKF, 其 中 设 q., 
Q. aÍ 和 9, 为 1.5- 图 18 -23 和 图 18 -24 中 分 别 给 出 了 RMS 第 卡 儿 和 球 坐 标 位 
置 误差 的 结果 ,结果 表明 ,对 于 笛 卡 儿 和 球 坐 标 EKF , 当 水 平 9 值 分 别 设 为 225 和 
550 时 ,跟踪 结果 最 好 。 
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图 18 -21 第 卡 儿 跟踪 算法 在 基准 目标 航 迹 中 的 RMS 笛 卡 儿 和 球 坐 标 位 置 误差 
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图 18 -22 球 坐 标 跟 踪 算 法 在 基准 目标 航 迹 中 的 RMS 笛 卡 儿 和 球 坐 标 位 置 误 差 


18.4.2.4 总 结 


对 于 3 个 不 同 的 目标 轨迹 ,机 动 加 速度 逐个 递增 。 考 虑 跟踪 算法 的 RMS 位 
置 误差 性 能 ,可 以 得 出 结论 :如 果 能 正确 选择 gq 的 分 量 , 那 么 在 机 动 目标 跟踪 
中 ,新 的 球 坐标 恒 速 度 动态 模型 优 于 笛 卡 儿 坐 标 便 速 度 模型 .而 且 , 当 目标 接 
近 距 离 RR 很 小 的 区 域 时 ,新 的 跟踪 器 表现 出 性 能 的 改善 ,特别 是 对 于 方位 和 高 
程 的 估计 。 
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图 18 -23 在 基准 航 迹 中 ,作为 9 的 函数 ,RMS 笛 卡 儿 位 置 误差 性 能 比较 


e 
UY m OO Adre 


E- 
RMSE Jj fit /(^)C-EKF 


RMSE Rn mile C-EKF 
RMSE 高 程 /(*)C-EKF 





50 100 150 200 
时 间 /s 时 间 /s 时 间 /s 


==-g =225; 4 


-.- q = 550. 


RMSE An mile S-EKF 





RMSE Jj {it /(°)S-EKF 
RMSE MFE /(°)S-EKF 





50 100 150 200 50 100 150 200 


B Bj /s 时 间 /s 时 间 /s 
图 18 -24 在 基准 航 迹 中 ,作为 9 的 函数 ,RMS 球 坐 标 位 置 误差 性 能 比较 


18.5 ”观测 数据 与 展望 


50 100 150 200 


这 个 案例 研究 被 认为 是 一 个 新 的 恒定 球速 度 动态 目标 运动 模型 ,可 以 将 合 
成 卡尔 曼 滤波 器 跟踪 算法 应 用 到 这 个 模型 中 。 通 过 对 3 个 仿真 航 迹 的 性 能 分 析 , 
目标 机 动 水平 逐 渐 增加 ,可 以 看 到 : 当 目 标 为 机 动 的 且 目 标 距 离 接近 零 时 , 球 坐 
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标 跟 踪 估 计 滤 波 器 的 性 能 更 好 。 结 果 还 表明 ,除了 S-LKF, 所 有 的 球 坐 标 跟 踪 估 
计算 法 的 性 能 相同 。 后 面 的 推导 可 得 出 结论 ,应 将 S - EKF 应 用 于 恒定 球速 度 跟 
踪 , 因 为 其 计算 时 间 最 短 。 

在 这 个 案例 中 ,由 于 空间 和 时 间 的 限制 ,本 书 未 能 充分 探究 新 的 可 能 的 动态 
模型 ,以 上 研究 可 能 包括 以 下 有 意义 的 拓展 : 

(1) 进一步 研究 混合 高 gq 模 型 和 低 g 模 型 的 球 坐 标 IMM 滤波 器 的 可 能 应 用 
领域 ; 

(2) 进一步 研究 距离 变化 率 观 测 值 对 球 坐 标 滤波 器 和 笛 卡 儿 滤 波 器 性 能 的 
影响 。 


附录 18. A 三维 恒定 角 速 率 偏转 运动 动力 学 


本 节 提 出 一 种 生成 航 迹 的 方法 , 航 迹 中 包括 水 平和 垂直 方向 恒定 转弯 速率 
的 转弯 .通过 该 方法 ,可 生成 包含 水 平 转弯 .垂直 俯冲 和 盘旋 的 运动 对 象 航 迹 。 

假设 目标 在 :时 刻 以 水 平方 向 鼠 (t) 和 垂直 姿势 y(t) 运动 。 采 用 图 18 -25 中 
的 向 东 — 向 北 - 向 上 第 卡 儿 坐标 系 ,这 里 东北 向 的 飞机 是 水 平 飞 行 的 飞机 。 


Z( L) 


(XY ZR, 6.6) 





Y (FR) 
图 18 -25 X -dt - 上 笛 卡 儿 坐标 系 
18. A.1 恒定 角 速 率 偏转 运动 速度 分 量 
可 将 恒定 速度 v 在 东 - db - 上 第 卡 儿 坐 标 系 中 的 笛 卡 儿 速 度 分 量 表示 为 


v, (1) using (t) cosy( t) 
v(t) = Ë: = eese 
v, (t) vsiny( t) 


AP: O A y 分 别 为 前 进 方向 - 180° < Ə < 180° 和 姿势 - 90° < y < 90°. 
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(18 - 113) 





对 于 一 个 恒 速 转动 ,参见 图 18 - 26, 可 以 有 
H(t) = H(t) + (1-1) & Ó, + QT (18 - 114) 
y(t) = y(t) +Wt-t) À y, + VT (18 - 115) 
eT At -ty HOt) >D, yl to) > Yoo FP Qf VAR RAF ASE Ër y 
向 的 恒定 角 速 率 。 需 要 注意 : 当 相对 运动 方向 向 右 转 弯 时 ,12 为 正 向 ; 向 上 转弯 
时 , 亚 为 正 向 ,将 式 (18 - 114) 和 式 (18 - 115) 代入 式 (18 - 113) ,得 
| Ee + {2T)cos(Yo + VT) 


b, (t) vcos( 9, + QT)cos(y, + WT) (18 — 116) 
v,(t) vsin(y, + VT) 
Y (4b) ZCE) 
yr) 
D y(t) 
Xk) R，( 水 平 距离 ) 


图 18 -26 在 水 平和 垂直 方向 以 恒定 角 速 率 飞行 的 飞机 


用 角度 和 的 正弦 和 余弦 三 角 等 式 对 w (0) 进行 展开 ,可 以 将 速度 的 分 量 写 为 
v,(t) = vsin( 9 + QT)cos( ya + VT) 
= v| sind ,cosQT + cosd,sinQT | [ cosy,cosWT - sinyosin YT] 
= vsind,cosy,cosQTcos VT — vsindosinyocosf2Tsin VT + 
vcosd,cosy,sinf)Tcos VT — veosd,siny,sinQTsinWT (18 — 117) 
用 式 (18 - 113) PHE v 的 定义 ,将 上 式 变 为 
s (4) = v,(t))cosQTcos¥T + v, (t,) sinf)TcosVT — 
v, (to) LcosdysinQTsinVT + 
sind, cosQTsin VT | (18 - 118) 
用 乘积 一 求 和 三 角 人 恒等式 ” ,将 上 式 改 写 为 


v (t) = Jr ln. (4) cost, T + v, (ty ) sing, T 一 


v, (ty) [ cosQ cosé T — sinB,sin£, T] + 
v, (t5) cos£, T + v, (ty) sin£,T + 
v, (ty) [ cosB,cos£, T — sin, sinZT | | (18 - 119) 
& 40-v (18 - 120) 
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& á 0 + W (18 - 121) 


类 似 地 ,得 
b, (t) = jl - v, (ty) sin£, T + v,(t,) cos, T + 
v, (ty) [ cosO,sin£, T + sinB,cosz, T] - 
v, (ty) sin£;T + v, (t9) cos€,T 一 
v, (15) [ cosd,sinZ, T + sind, cos£, T] | (18 - 122) 
计算 v. (10) ,有 
v,(t) = vsin(y, + VT) 
= vsiny,cos¥T + vcosy,sin VT 
= v,(ty) cosVT + vcosy,sin VT (18 - 123) 
4 


7 2 
b, (t) = /wu (t) +u (to) 
= Wo sin pacos yo + v cos S, cos y, 


= vcosyo (18 - 124) 
则 式 (18 - 123) 变 成 
v,(t) = v,(to)cosWT + v,(t,)sinWT (18 - 125) 
定义 
c; À cos(£,T) i =1,2 (18 - 126) 
s, A sin(£ T) i =1,2 (18 - 127) 


可 以 将 式 (18 - 119) 和 式 (18 - 122) 重 写 为 
v(t) = FH Cei to) (ty) + Ges) Qs) + 
[ (s, — ss )sin@, — (e, — 2) cosy ]v,(t))} — (18 - 128) 
v (1) = >i- (s, tJl) + (a, ev, (8) + 


[ (c; — c,)sinƏ, + (s, — 5,) cos Jv, (to)} (18 - 129) 
但 是 ,必须 考虑 有 些 特殊 情况 : 
(1) SN = 多 =0 的 情况 .此 时 c = 1 Hs, = 0(i =1,2) ,得 v(t) = v(t). 
(2) 4N=0,¥#0,é, =- VH £, = 几时 ,得 cl = c, = cos( VT), Hs, = 
— sin( WT) ,s, = sin( VT) , 
(3) XY =0,0=0 HE, =£, = 有 时 ,得 5 = e, = cos( NT), Hs, = s, = 
sin( QT) 。 
(4) WRN = < 0,82, =0,c = 1 Ms, = 0, 
(5) 如 果 2 = - FX 0,48 £, =0,c = 1 fils, = 0, 
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18. A.2 恒定 角 速 率 偏转 运动 位 置 分 量 
为 生成 位 置 分 量 表达 式 r(t) ,已 知 
r(t) = r(t) + [vG0 du (18 - 130) 
考虑 积分 的 形式 , 即 
[edu " [eos Cé Cu - to) )du 
š; | Leos CE) cost - Et) — sin(£,u)sin( — £45) ] du 


= cos( - £t) | cos £u) du - sin( - £to) | sin(&u)du 


= Esin(éit) 
i zu (18 — 131) 
类 似 地 ,有 
[ sdu = [ sinc Co <h) da 
= gC cen) 
= L(1-«) (18 - 132) 
定义 | 
kà zu (18 - 133) 
h A x e) (18 - 134) 


在 执行 积分 之 后 , 式 (18 - 130) 的 分 量 由 下 式 给 出 , 即 
FF 二 arts} + Fila + ga), (to) + (hy + h,)v, (to) + 
[ (h, - hy) sind, - (g, — gz) cos |v, (ty) | (18 — 135) 


r(t) = r,(&) + - (hy & h)v (ty) + (g, +82) 0, (to) + 


[ (g, - g) sind + (h, — h;)cosO, |v, (ty) | (18 — 136) 
r,(t) = r,(to) + m x * a e) (18 - 137) 
考虑 以 下 特殊 情况 : 
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(1) KEN = v =0 AF v(t) = v(to), 所 以 有 r(t) 2 r(1) +Ty(to)o 

(2) í Q = Two B = g, = sin(VT)/W fll h, =- (1 - 
cos( VT))/W,h, =- hio 

(3) 4 @' = 4.0 » 9M. 得 g = g, = sin(QT)/N fü h, = h, = 
- (1 - cos( QT) )/02, 

(4) WRN = Z 0,#8 £, = Og, = THA, = 0, 

(5) MRQ=-WF0,FéE, = 0.z, = 1 #l h, = 0, 


18. A.3 联合 轨迹 转移 方程 


将 式 (18 -135) ~ 式 (18 - 137) 式 (18 -128) 式 (18 - 129) 和 式 (18 - 125) 
合并 为 一 个 矢量 和 矩阵 方程 ,得 
x(t.) = /.(XCt)) (18 - 138) 
£(L) & K. = [z. . P. eias, Mi le (18 - 139) 
通过 设置 一 个 航 途 基准 点 ,将 航 迹 划分 成 段 ， 并 指定 每 段 的 转弯 和 俯冲 速率 ， 可 
用 式 (18 - 138) 来 生成 真实 航 迹 。 对 于 这 一 案例 中 用 到 的 所 有 仿真 数据 ,生成 航 
迹 的 时 间 间 隔 是 0. 1s 或 者 采样 频率 为 10Hz。 


18. A.4 “基于 期 望 偏 转 加 速度 来 设置 偏转 速度 


对 于 速度 " 恒定 的 目标 ,其 速度 分 量 是 飞行 方向 和 姿势 的 函数 ,由 式 (18 - 113) 
给 出 ,加 速度 可 以 表示 成 


à (t) 
a(t) = — = Ë | 
a,(t) 


cos (t) cosy(t) 一 ysind ( t) siny(t) 
= U = SsinScosy(1) - ycosd ( t) siny( 1) (18 - 140) 
ycosy(1) 





加 速度 的 大 小 由 下 式 给 出 , 即 
| a(t) | = Mai(t) *a;(t) +a:(t) (18 - 141) 


a(t) = v [ 9? cos (1) cosy(1) + y sin’ 9(£)sin^y(1t) - 


28 ycosd(t) sin& (t) cosy( 1) siny(t) ] (18 - 142) 

a(t) = v [&sin'O(1)cos"y(t) + y'cosO(r)siny(t) + 
2óycosO (1) sind (t) cosy(t) siny(t) ] (18 - 143) 
a2 (t) = v y'cosy(t) (18 - 144) 
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因此 ,有 


| a(t) | = v V @?°cosy(t) + y (18 - 145) 
HÒ = Q #ly = 多 ,上 式 可 变 为 
| a(1) | =v /(Peosy(t) + V^ (18 — 146) 
考虑 以 下 恒 速 率 转弯 / 俯冲 特殊 情况 : 
(1) 对 于 水 平 转弯 ,7y(t) — y, Y — 0 H 
| a(t) | = fcosyo (18 - 147) 
(2) 对 于 垂直 俯冲 :2 一 0 H 
|l a(t) | = Ww (18 - 148) 


假设 指定 一 个 水 平 转弯 ,其 加 速度 定义 为 重力 加 速度 g 的 倍数 ,如 x g。 对 于 
一 个 初始 姿势 y。= 0 的 水 平 转弯 ,有 


0 = — (18 - 149) 


例如 ,指定 一 个 以 600kn 的 速度 匀速 运动 的 目标 ,以 6g 加 速度 水 平 向 右 转弯 , 转 
弯 速 率 应 该 设 为 


6 - 
Üa == x € (18 - 150) 
这 里 转换 因子 C 定义 为 
a LL. 04(vs)g] (18 - 151) 


= (6076. 11549n mile/ft) /3600( s/h) ) 
因此 ,对 于 这 种 情况 ,必须 设置 2 = 0. 19rad/s, 以 得 所 需要 的 转弯 加 速度 。 


附录 18.B ”三维 坐 标 变换 


当 使 用 一 个 跟踪 滤波 器 ,定义 笛 卡 儿 坐 标 系 下 状态 矢量 和 球 极 坐标 系 下 观 





换 。 这 些 转换 必须 应 用 到 变换 后 的 矢量 以 及 协 方差 矩阵 当中 。 本 节 将 导出 这 些 变 
换 , 并 给 出 如 何 开 发 子 程序 以 实现 这 些 变换 。 作 为 附加 好 处 ,可 以 生成 变换 的 雅 
可 比 和 矩阵 ,并 成 为 输出 的 一 部 分 。 

给 出 多 维 坐 标 变换 的 一 般 概 念 。 由 一 个 坐标 系统 变换 至 另 一 个 系统 的 矢量 
变换 可 以 写 为 

r = f(x) (18 - 152) 

式 中 :zx 为 原始 坐标 系 中 的 矢量 ;r 为 变换 后 坐标 系 中 的 矢量 。 

为 给 出 协 方差 矩阵 的 变换 ,检验 协 方差 矩阵 在 变换 后 系统 中 的 定义 ,有 
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P" = el(r-r)(r -r)!| (18 - 153) 
这 里 代表 期 望 值 ,r = e(r) ,由 式 (18 - 152) 得 
r = f(x) (18 - 154) 
将 f(x) 展开 为 关于 x 的 一 阶 泰勒 级 数 ,由 式 (2 - 70) 得 
f(x) = Lf) 1, + [Vef (x) 11 (x - x) 


= f(x) + F(x - 2) (18 - 155) 
这 里 将 雅 可 比 矩 阵 五 定义 为 
af af A 
Ox, OX, Ox, 
B th dh 
F = [V.f'(x) Ji; =| 0% 9x, ax, (18 - 156) 
Ym fm fn 
Ox, Ox; 0x, . s 


AP:n HRE x 的 维 数 ;m 为 矢量 了 的 维 数 。 
将 式 (18 - 155) 代入 式 (18 - 153) ,得 


P” = el[ f(x) -f(x)][f(x) -f(x) J"! 
= el (f(x) + F(x - £) - f(x) JU) + F(x - x) - f(x) ]"| 
= Fel(x - x)(x - x)' | F" 


= FP™F" (18 - 157) 


式 (18 - 157) 给 出 了 一 阶 协 方差 矩阵 坐标 变换 的 一 般 方 程 。 注 意 , 式 (18 - 157) 
包括 雅 可 比 矩 阵 的 计算 ,坐标 变换 子 程序 同时 给 出 雅 可 比 和 矩阵 。 


18. B.1 笛 卡 儿 坐 标 系 变换 为 球 坐 标 系 
如 图 18 - 25 所 示 ,对 于 一 般 的 三 维 坐 标 系统 ,可 以 将 状态 矢量 写 为 笛 卡 儿 
坐标 系 形 式 , 即 
x = [rv sr, tyrum]? (18 - 158) 
或 者 写 为 球 极 坐 标 系 形式 BD 
r = [R,R,0,0,0,0]" (18 - 159) 


由 图 18 - 25 ,可 知 从 笛 卡 儿 坐标 系 一 一 球 坐 标 系 的 位 置 坐标 变换 为 
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R 
中 tan( ==.) (18 — 160) 


tan( 7 " =] 
取 每 个 分 量 关 于 时 间 的 导数 ,得 球速 率 分 量 为 


TU, t TU, tro, 











R 
R 
FE. f +s 
0 |= UN (18 - 161) 
$ Rv, 一 r,R 
RR, 


这 里 水 平 距离 定义 为 R À Vr + 下。 在 三 维 坐 标 系 中 ,R 通常 是 指 斜 线 距 离 。 这 
RE SEE RU fF JL AS ER IR 一 一 球 坐标 系 的 矢量 变换 h(x) 定义 为 





Mr + +r, 
TV, + ro, +7,0, 
R h, (x) " 
i r 
R h,(x) arctan( =) 
0 y 
r=| |=h(x) A h, (x) = ru, — rV, (18 — 162) 
0 h,(x) R? 
H 
Ó hs(x) r, 
Rv, - r,R 
RR, 


由 式 (18 - 156) , A) 1388 F ULAR b A 一 一 球 坐 标 系 协 方差 变换 的 雅 可 比 
B S 
ah, ah, ah, 
ðr, dv, — ë, 
ðh, ðh, ðh, 
ar, ov, a, (18 - 163) 








=> 
II 

II 
Q 
* 


dhe aig Oh, 


ar, av, Ow, 


x=x 
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如 果 先 计算 式 (18 - 162) ,在 每 步 计算 中 可 以 使 用 球 坐 标 分 量 , 得 雅 可 比分 量 为 


Hi 


Hn 





LI +R €] = 


ðr, 
H, = H, = 0 
Ty 
R 
Ta 
R 
Rv, 一 rR 
21 = R: 
2 = H, 
Rv, — r,R 
H,, = R i 
Hy, = H, 
Rv, - r,R 
A,, = R 
Hy, = Hy 
r, 
H, = R: 





r 


x 


R 


(18 — 164) 
(18 - 165) 
(18 - 166) 
(18 - 167) 
(18 - 168) 
(18 - 169) 
(18 - 170) 
(18 - 171) 
(18 - 172) 
(18 - 173) 
(18 - 174) 
(18 - 175) 
(18 - 176) 
(18 - 177) 
(18 - 178) 
(18 - 179) 
(18 - 180) 
(18 - 181) 
(18 - 182) 
(18 - 183) 
(18 - 184) 
(18 - 185) 
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N, ==" perk - Ro) + Cr R — Ro.) | (18 - 186) 





RR, 
He = h (18 - 187) 
Hs = RR xl (rR Ro,) + zs Qr - f )] 
Ha = Ms (18 - 188) 
He = x [ (22 + R2)R - r,Ro, ] (18 - 189) 
Ho = Hs (18 - 190) 
利用 式 (18 - 157) ,可 以 将 笛 卡 儿 坐 标 系 一 一 球 坐 标 系 的 协 方差 矩阵 变换 写成 
P” = HP"H' (18 - 191) 


式 中 : 忆 A P SP Sill NB LAR b TIPRAe bA F BU 2858 £ s 
18.B.2 ” 球 坐 标 系 变换 为 笛 卡 儿 坐 标 系 


有 时 需要 进行 式 (18 - 162) 的 逆 运 算 , 即 球 坐 标 系 一 一 笛 卡 儿 坐 标 系 变换 
的 定义 为 


x - g(r) (18 - 192) 
首先 检验 位 置 分 量 ,使 用 图 18 - 25,18. 
r, Rsin0cos$ 
B = [e (18 = 193) 
r Rsing 


取 每 个 分 量 的 导数 ,得 
U. 


v 


y 


Singcos 中 Rcos@cosb  - Rsindsind R 
= me - Rsin@cosh  - M 8| (18 - 194) 
中 











v, sinó 0 Rcosó 
合并 式 (18 - 193) 和 式 (18 - 194) ,得 球 坐 标 系 —— 笛 卡 儿 坐 标 系 的 变换 为 
r, g(r) Rsin@cosh 
v, g(r) RsinOcos + GRcos6cosó — jRsin0sin$ 
In |. | ar) | _ Rcos6coso 
dic v, I g,(r) I Reos0cos$ - ORsinOcosd - 中 Recosbgsin 由 
r, gs(r) Rsing 
v, g (r) Rsind + bReosd 
(18 — 195) 


给 出 协 方差 矩阵 由 球 坐 标 系 一 一 FAB ILA ER BA 
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P™ -6PG 


式 (18 - 156) 的 雅 可 比 变换 分 量 为 


S 
t3 
> 


= Singcos 中 

= Gis = Gis =0 
= Reosécosh 

= — Rsin@sind 


gcosbcos 由 - dsinOsind 


C, 

Gn 

G1 

Gis 
Cn 
ReosOcos 
Gi 


一 bRsingcos 由 一 中 Rcosbgsin 中 


- Rsinbsin 由 - ORcosOsind 一 中 Rsingcos 中 


Gis 


cosécosh 


G 
Gy = Gy = 6, = 0 
G 


3 = 一 人 singcos 中 
G4, = — Reos@sind 


— gsingcos 中 — 中 cosbgsin 由 


Ga 


- Rsin6cosó — gRcosgcos 由 + $Rsin0sin$ 


G33 


- ReosOsing + ORsinOsind - bReosOcosd 


Gss 


= sin$ 

= G, = G, = G, = 0 
= Reosd 

= $cosó 

= Gs 

= Ga = 0 


Reosó 一 pbRsing 
= Gss 


(18 - 196) 


(18 - 197) 
(18 - 198) 
(18 - 199) 
(18 - 200) 
(18 - 201) 
(18 - 202) 
(18 - 203) 
(18 - 204) 
(18 - 205) 
(18 - 206) 
(18 - 207) 
(18 - 208) 
(18 - 209) 
(18 - 210) 
(18 - 211) 
(18 - 212) 


(18 - 213) 
(18 - 214) 


(18 - 215) 
(18 - 216) 
(18 — 217) 
(18 - 218) 
(18 - 219) 


(18 - 220) 
(18 - 221) 
(18 - 222) 
(18 - 223) 
(18 - 224) 
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第 19 章 
利用 摄影 测量 法 跟踪 下 落 刚体 





19.1 概述 


摄影 测量 法 属于 数学 科学 范畴 , 即 : 用 多 个 摄像 机 获取 二 维 视频 图 像 帧 数据 
序列 ,从 中 观测 得 一 组 目标 特性 ,在 此 基础 上 估计 目标 的 姿势 (三 维 位置 和 方 
向 ) 和 运动 属性 (平移 旋转 的 速度 和 加 速度 ) 。 人 们 提出 许多 方法 来 求解 这 个 估 
计 问 题 ,1990 年 之 前 最 常用 的 是 非 线 性 最 小 二 乘法 (nonlinear least squares, 
NLLSQ) 方法 ,Huang 和 Netravali 在 文献 [1] 中 对 早期 的 NLLSQ 方法 进行 了 综 
iB, TE NLLSQ 方法 的 每 个 时 间 步 中 ,用 预测 视频 图 像 与 当前 实际 视频 图 像 之 间 
的 残 差 来 进行 一 个 迭代 的 NLLSQ 过 程 ,使 得 局 部 残余 误差 最 小 化 ,进而 得 到 刚 
体 状 态 估 计 。 另 一 种 更 为 现代 的 算法 是 贝 叶 斯 估计 扩展 卡尔 曼 滤波 器 (EKF ) 方 
法 ,由 lu 和 Wohn"! 和 Gennery?! 提出 , 并 得 了 非常 好 的 试验 结果 。 其 后 ， 
Welch'*?! 提出 一 种 SCAAT EKF 方法 。 

上 面 提 到 的 方法 都 有 一 些 缺 点 ,会 影响 刚体 状态 估计 性 能 ,NLLSQ 方法 假 
定 动态 模型 中 没有 噪声 ,在 时 间 上 不 递归 ,这 样 每 一 次 迭代 都 需要 计算 雅 可 比 和 矩 
阵 ( 偏 导数 矩阵 ) ,观测 值 集合 由 视频 帧 图 像 组 成 ,每 次 迭代 都 需要 重新 处 理 的 
所 有 图 像 帧 ,因此 该 方法 非常 耗 时 。 而 且 , 因 为 应 用 于 摄影 测量 中 的 NLLSQ 是 一 
种 ”一 次 一 步 ” 方 法 ,在 NLLSQ 中 不 估计 平移 速度 和 角度 变化 率 ,必须 使 用 后 处 
理 平 滑 步 骤 单 独 计算 。 

为 了 使 观测 过 程 线 性 化 ( 见 第 7 章 ) ,每 个 时 间 步 必须 计算 非 线性 观测 模型 
和 动态 转移 模型 的 雅 可 比 矩 阵 ,因此 EKF 方法 很 难 实现 ,已 经 证 明 , 对 于 很 多 高 
度 非 线 性 问题 ,这 种 解析 线性 化 有 很 多 不 足 。 

为 进行 安全 性 评估 ,本章 致力 于 研究 估计 和 跟踪 中 的 特殊 问题 一 一 估计 和 
跟踪 从 飞机 外 挂 架 上 落下 的 刚体 (看 似 炸弹 的 目标 ) 随时 间 变 化 的 姿势 .这 里 所 
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使 用 的 图 像 序列 是 由 多 个 附属 于 同一 参考 目标 的 高 速 图 像 传 感 器 记录 得 .用 图 
像 传 感 器 或 摄像 机 可 靠 观测 跟踪 目标 的 刚体 状态 轨迹 .图 19 -1 和 图 19 -2 给 出 
标记 在 参考 目标 和 刚体 目标 上 的 特征 点 。 在 飞机 上 共 装 有 19 个 摄像 机 ,其 中 辟 
尖 上 面 有 4 个 摄像 机 ,飞机 尾部 下 面 4 个 摄像 机 ,机 头 下面 1 个 摄像 机 。 每 个 摄像 
机 的 图 像 帧 中 ,特征 点 的 位 置 都 记录 在 一 个 时 域 图 像 序 列 当 中 ,用 于 估计 跟踪 目 
标的 航 迹 。 每 个 图 像 传感器 由 特定 视角 记录 目标 运动 ,得 到 各 自 独 特 的 图 像 序 
列 。 每 个 图 像 序 列 必须 适当 地 结合 其 它 摄 像 机 的 图 像 序列 ,在 每 个 时 间 步 计算 航 
迹 的 最 佳 跟 踪 估 计 。 跟 踪 目 标的 航 迹 可 以 用 来 进行 以 安全 为 目的 的 脱 靶 量 分 析 ， 
或 对 其 他 类 型 的 建 模 与 仿真 进行 验证 和 改进 。 因 为 图 像 传感器 估计 3D 空间 中 刚 
体 目标 随时 间 变 化 的 位 置 和 方向 ,将 摄影 测量 法 估计 用 于 准确 近 距 离 跟 踊 是 首 
rR., 





图 19 -1 美国 海军 F - 18E 超级 大 黄蜂 式 飞 机 准备 投放 4 个 Mk - 62 水 雷 





图 19 -2 由 安装 在 飞机 尾部 下 面 的 摄像 机 拍摄 到 的 投放 MK - 62 的 特写 镜头 


在 估计 过 程 中 ,本 书 提出 两 种 新 方法 , 即 无 迹 卡 尔 曼 滤波 器 (UKF) 和 广义 
UKF 粒子 滤波 器 (分 别 参见 第 9 章 和 第 17 章 ) ,因为 这 些 滤波 器 能 够 估计 刚体 运 
动 状态 ,包括 笛 卡 儿 速 度 和 加 速度 以 及 角速度 和 角 加 速度 ,而 无 需 计 算 雅 可 比 矩 
阵 , 因 此 性 能 优 于 EKF 和 NLLSQ。 

第 19. 2 节 提 出 一 种 动态 运动 模型 , 以 描述 下 落 刚 体 目标 的 平移 和 旋转 运 
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动 . 该 节 还 讨论 了 动态 噪声 项 ,用 来 描述 动态 模型 中 微小 的 运动 偏离 .第 19.3 节 
介绍 了 观测 模型 ,首先 计算 刚体 表面 特征 点 的 位 置 ,将 刚体 上 的 三 维特 征 点 由 本 
体 坐 标 系 变换 ( 旋转 和 平移 ) 至 参考 (飞机 的 ) 坐标 系 ; 然 后 将 三 维 点 变换 至 摄 
像 机 的 坐标 系 并 将 三 维 点 投影 到 摄像 机 的 二 维 图 像 平面 。 

第 19. 4 节 讨 论 了 所 使 用 的 估计 方法 ,第 19. 4. 1 节 为 NLLSQ ,第 19. 4.2 节 为 
UKF ,19.4.3 Jj UKF 组 合 粒子 滤波 器 ,第 19. 4.5 节 给 出 所 有 滤波 器 的 初始 值 。 

为 进行 性 能 分 析 ,给 出 一 种 综合 刚体 模型 和 综合 运动 模型 ,以 模拟 刚体 由 飞 
机 外 挂 架 投 放 后 的 运动 。 第 19.5 节 用 这 个 模型 和 方法 来 生成 模拟 图 像 帧 数据 。 
第 19. 6 节 给 出 所 有 估计 方法 相关 性 能 ,并 进行 了 重要 结果 总 结 。 

附录 19. A 概括 介绍 了 一 些 定义 和 相关 数学 概念 ,包括 四 元 数 、 轴 角 矢 量 和 
旋转 等 。 


19.2 ”刚体 运动 的 过 程 模型 


这 里 运动 模型 为 时 间 函 数 ,描述 刚 体 的 平移 和 旋转 运动 ,假定 一 个 瞬时 状态 
At x(t) = [p'(t),a'(t)]" 定义 了 相对 于 固定 参考 笛 卡 儿 坐 标 系 中 图 像 帧 的 
刚体 重心 的 平移 p(t) = [x(t) ,y(t) ,z(t)]', 并 用 一 个 轴 角 矢量 alt) = 
[a, (t) ,a,(1) a, Ct) |" 来 表示 刚体 相对 于 参考 帧 的 方向 。 在 某 些 条 件 下 ,矢量 a 
可 能 变 得 数值 上 不 稳定 (尤其 是 当 |a | = 2mn 时 ,其 中 为 正 整 数 ) ,所 以 用 单 
位 四 元 数 g = [q,,4..4,.4.]. Fa 结合 来 代表 方向 ,在 第 19.2.2 节 和 附录 
19. A. 2 中 ,详细 讨论 了 4 值 的 计算 ,在 后 面 给 出 的 状态 模型 (过 程 ) 中 ,将 调整 状 
ARE, MEERE .加 速度 矢量 和 加 加 速度 (三 次 时 间 导 数 ) 项 来 控制 平移 和 
旋转 运动 。 

因为 刚体 的 平移 运动 与 旋转 运动 无 关 , 将 分 别处 理 这 两 种 运动 ,在 下 面 章 
节 将 给 出 动态 转移 方程 ,将 上 面 定 义 的 刚体 状态 矢量 从 时 刻 c. 转移 至 之 后 
的 已。 在 第 19. 2. 1 节 ,用 转移 方程 控制 状态 矢量 的 平移 部 分 ,在 第 19.2.2 节 ， 
用 转移 方程 处 理 状 态 矢 量 的 方向 部 分 。 一 般 来 说 ,可 以 将 总 体 转移 方程 写成 如 
下 形式 , 即 

x, — f(x, 4,9.) (19 21) 
XB iv, a 为 驱动 过 程 的 噪声 .在 所 有 情况 下 都 把 噪声 视 为 高 斯 加 性 噪声 , 这样 
式 (19 - 1) 变 成 

x. = f(x. y) + Ves (19 -2) 
第 19. 2. 3 节 深 入 讨论 了 这 种 组 合 的 姿势 转移 模型 。 第 19.2.4 节 将 讨论 ww AR 
声 模型 的 性 质 。 


19.2.1 刚体 平移 运动 的 动态 转移 


Ap, 为 一 个 三 维 矢量 ,代表 t, 时 刻 刚体 重心 (center of gravity,cg) 的 笛 卡 
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SLA BAER Bp, = [x,,y,,z,] 。 将 p(t,) 展开 为 关于 4_, 时 刻 的 时 域 泰勒 展开 
式 , 有 


P. = Put [P]. 6 -1,4) + [a (ty = tra)? 


2! 
d'p(t) (t, -t,.,)° 
[^ Nar ms. (19 - 3) 
= Pi. + Tp + 3 Pai + pP po (19 - 4) 


这 里 p、p Alp 代表 前 三 阶 位 置 的 时 间 导 数 ,7, A t, -t,_,。 这 个 例子 中 ,假设 所 有 观 
测 摄像 机 同步 记录 图 像 ,那么 可 假定 T, 为 常数 T。 仅 保留 至 三 阶 项 ,将 式 (19 - 3) 
改写 为 
Ps. = Pra T Tp, * T$ * T$, , (19 — 5) 
对 于 恒定 位 置 模型 ,pp 和 p 均 为 零 , 因 此 定义 刚体 的 零 阶 三 维 状 态 和 撩 量 中 
仅 包含 位 置 分 量 p," =p, = [x(t) ,y(t) ,z(t)]", 式 (19 - 5) BH 
P = p." + v) (19 - 6) 
这 里 增加 了 一 个 零 阶 过 程 速 度 噪声 项 vy," 。 增 加 过 程 噪声 是 为 了 表示 模型 中 由 
于 一 些 未 建 模 因素 如 风力 、 摩 擦 等 导致 的 目标 位 置 的 微小 偏差 。 
基于 速度 恒定 假设 ,定义 一 个 包含 位 置 和 速度 分 量 的 6 维 状态 矢量 p'”= 
[Pipi], WRAS — 5) 变 为 一 阶 过 程 模型 B 


p = pp evt (19 -7) 
soe m 

F = | | (19 - 8) 
0, I, 


stp M 0, 431] = HER [LX ACRI XR IAE Lv U 为 一 阶 加 速度 过 程 噪声 。 
采用 相似 的 形式 ,对 于 加 速度 恒定 模型 (19 - 5) ,9 4E p, = [p!, 
Pp',p'] ”和 过 程 模型 变 成 


p? = Fp? ev? (19 -9) 
E-E T 

(2) _ = 

Fe" = 0, L TI, (19 - 10) 
0, 0, 1, 


式 中 :v21 为 二 阶 加 加 速度 过 程 噪声 。 
对 于 恒定 加 加 速度 的 12 维 模型 ,p。 = [p,.D,.D. D. ,进而 得 过 程 模 
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型 为 


p? = FOp Y. (19 - 11) 
E. = 
I, TI, vb vh 
T 
per 一 0, I, TI, ah (19 = 12) 


0, 0, 0, LZ 
step ivi* 为 三 阶 snap( gk jounce) 四 阶 导数 过 程 噪声 。 


19.2.2 刚体 旋转 运动 的 动态 转移 


可 以 用 多 种 方式 表示 刚体 的 瞬时 方向 。 这 个 方向 可 以 表示 为 一 组 相对 于 3 
个 稍 卡 儿 参 考 帧 轴 的 欧 拉 旋 转角 。 这 里 选择 的 右手 笛 卡 儿 坐标 系 参考 帧 是 x fh 
指向 前 方 ,y 轴 当 向 前 看 时 水 平 向 右 ( 右 航 ) ,z 轴 竖 直 向 下 。3 个 方向 角 为 1 中 ,9， 
y) ,其 中 由 ( 翻滚 ) 表示 相对 于 x HAW Ff (counterclockwise, CCW) 旋转 ,6( 俯 
仰 ) 表示 相对 于 y 轴 道 时针 旋转 ,gp( 偏转) 表示 相对 于 z 轴 逆 时 针 旋 转 。 但 是 , 利 
用 欧 拉 角 描述 刚体 方向 在 某 些 情况 下 会 导致 奇异 点 ”' 。 


19.2.2.1 旋转 表达 式 之 间 的 转换 


将 a = [a,,a,,a,]' e R? 定义 为 旋转 的 轴 角 表示 ,这 里 a 的 方向 指定 旋转 轴 
的 角度 , |a || = Jal + a; + a; 表示 旋转 角度 ( 用 弧度 表示 ) 。 
Sq = [9g,,9;,9,,9:] © 如 ,其 中 五 是 四 元 数 空间 ,定义 为 旋转 的 单位 四 元 数 
表示 。 这 里 q, 为 实数 ,gq, q, fil q, 为 纯 虚 数 系数 ,并 有 目 满足 Vg; + q: +q. *q. = 1, 
令 M e R x R 为 一 个 3 x 3 标准 正 交 旋转 矩阵 。 
为 将 轴 角 矢量 a 转换 为 单位 四 元 数 4 ,定义 函数 Q: R? — HWA 
. Weil 1 


sin 
q = Q,(a) = "-—i : [a, ,a, ,a, | (19 - 13) 


2 ` lal 








为 将 单位 四 元 数 q 转换 为 角 轴 矢量 e, 定 义 函 数 A, : H — R , 则 有 


2cos !q, 
= Te a al (19 - 14) 


ÀÁA-q C 





a = A(q) 


19.2.2.2 四 元 数 乘法 


设 q,.q, fil q, 为 四 元 数 ,四 元 数 乘法 q, = 919. 定义 为 
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qs.. Qi. Zis Qh QI. 
d, Q5.« = Qi, qi, — diš qi.y 92,x (19 - 15) 
qs.y qi iy qi.: q... Qi. q5., 


qs.: Qi. 一 qi diz q... q».. 
19.2.2.3 方向 的 动态 模型 


利用 轴 角 表达 式 来 表示 刚体 方向 的 动态 时 间 转 移 。 轴 角 方 向 矢量 a, 表示 相 
对 于 之 前 方向 的 一 个 微小 旋转 , 即 从 刀 - 到 4 时 刻 的 旋转 。 将 这 个 微小 轴 角 旋转 
加 入 (通过 旋转 ) 明确 的 外 部 四 元 数 g, ,其 中 包含 用 在 观测 模型 中 的 i,_, 时 刻 
的 完整 旋转 。 注 意 ,a 及 其 导数 aa 和 a( 如果 存在 的 话 ) 都 是 相对 参考 坐标 系 帧 
而 不 是 本 体 坐 标 系 帧 ,因此 在 动态 模型 中 不 需要 出 现 g 值 。 
定义 状态 矢量 方向 上 的 轴 角 矢量 a 及 其 导数 ,恒定 方向 模型 中 a.a 和 4& 均 
为 零 , 因 此 将 零 阶 模型 的 状态 矢量 看 成 三 维 轴 角 矢量 a ,有 
a” = gu +p." (19 - 16) 
AP oi^) 为 方向 的 三 维 零 阶 角 速度 噪声 。 
对 于 恒定 旋转 速率 模型 ,选择 6 HEUS at 4 [ea,,a;] ,得 一 阶 过 程 模 
型 为 
a = gi (et) 4a. (19 - 17) 
式 中 :p 纪 为 方向 的 三 维 一 阶 角 速度 噪声 。 为 将 轴 角 矢量 a 在 时 间 区 间 T = t, - 
t, i 向 前 传播 ,不 能 简单 地 把 Ta,_, MA a, 来 得 一 个 新 的 矢量 a, ,因为 不 能 用 矢 
量 轴 角 表 达 式 相 加 来 得 两 个 三 维 旋 转 的 县 加 (或 组 合 ) 。 反 之 ,把 轴 角 矢量 变换 
为 单位 四 元 数 ,计算 它们 的 乘积 (记得 颠倒 它们 的 顺序 ) ,然后 把 结果 变换 回 轴 
角 矢 量 ( 或 者 ,可 以 用 旋转 矩阵 代替 单位 四 元 数 ) .具体 而 言 ,为 Aa 是 方向 的 递 
增 变 化 ,由 时 间 区 间 7 内 恒定 旋转 速率 产生 ,因此 有 


Aa = | ade = Ta (19 - 18) 

Hi qa, Fl qu, 定义 为 
qa, = Qalani) (19 - 19) 
qu = Q, (Aa) = Q,(Ta) (19 - 20) 

将 这 两 个 四 元 数 相 乘 ,记得 颠倒 它们 的 顺序 ,将 结果 变换 回 轴 角 矢量 a, , 即 

a, = Alaala, ) (19 -21) 

假设 转速 恒定 ,所 以 有 
a, =i (19 - 22) 


现在 ,将 式 (19 - 19) ~ 式 (19 - 22) 代入 式 (19 - 17) ,得 
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-| J A,( Q, (Ta, 4)9,(a,,)) +p) (19 - 23) 


a, = 
对 于 一 个 恒定 旋转 加 速度 模型 ,选择 9 维 方向 状态 矢量 a, A [al ,a, ,a,] ,得 
到 二 阶 过 程 模型 为 
a =" (a) +9 (19 - 24) 
式 中 :ps 为 方向 的 三 维 二 阶 角 速度 噪声 。 在 这 个 模型 中 ,时 间 区 间 7 方向 的 递 
增 变 化 为 


Aa = 人 (riDd = Ta + Tà (19 — 25) 

由 于 a fila 为 特征 矢量 ,所 以 有 
a, =a, + Ta, _, (19 - 26) 
a, = a, (19 - 27) 


因此 有 


a, 
= g) (a) +p? 
i 


A 9. (rà... + à, ,)Q.(@,.)) 


= (1) u 
9 HN + p) (19 - 28) 


à, 
对 于 一 个 恒定 角 加 加 速度 模型 ,选取 一 个 12 维 的 状态 矢量 as” A [a; ,a,"， 
a) a.) ,得 到 过 程 模型 为 
aO = g, (at ) + pe (19 pe 29) 
SOP pO) 为 这 个 模型 的 12 维 角度 噪声 ,并 有 


A o (Taz, + Tar, + Tar) 2.(a,.)) 


g(a) = al, + Tal, + >a) 


(19 — 30) 
19.2.3 组合 动态 过 程 模型 


平移 和 定向 过 程 模型 可 以 组 合成 一 个 更 通用 的 过 程 模 型 。 定 义 组 合 状态 矢 
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量 为 


(i) d D, _ 
x = Ls] (19 - 31) 
状态 噪声 模型 定义 为 
oy Te ; š 
c * sl (19 - 32) 
完整 的 动态 模型 可 写 为 
KU = f(a) s». (19 - 33) 
i) (i) e F” pi 
RO. (419) = le (al (19 - 34) 


19.2.4 动态 过 程 噪 声 模型 


假定 噪声 为 零 均 值 高 斯 分 布 , 可 以 将 噪声 的 形式 写 为 
^ N(0,,,, QP) š 43.1 23 (19 — 35) 
这 里 0, 是 一 个 3(i+3) x3(i +3) WAH. 
由 文献 [3,7] 可 以 发 现 ,恒定 位 置 /方向 模型 的 噪声 项 代表 速度 白 噪声 ,其 
连续 过 程 可 简化 为 








Q" = B TI, 0, | (19 - 36) 
0, qi TI, 
这 里 qi MO” 分 别 与 平移 和 旋转 速度 噪声 的 标准 偏差 相关 。 
对 于 恒定 速度 模型 ,文献 [3 ,7] 中 给 出 如 下 噪声 协 方 差 BI 
(1) 
Q” = B Q 4 | (19 - 37) 
0, qt" Q, 
at S 
Q, = (19 ~ 38) 
TI TI 
对 于 高 阶 位 置 方程 ,可 将 恒定 加 速度 动态 噪声 模型 "| 写 为 
Q” = whe 0, | | (19 - 39) 
I 0, q” Q, 
py m P 
20h gh gh 
tie EX = g 
Q, = 35 3b 5h (19 - 40) 
p T 
rac ais TI, 
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根据 文献 [8 ] ,将 恒定 加 加 速度 噪声 协 方差 写成 


Q? = GQ; O (19 - 41) 
n 0, q? Q, 
T Wes. ue y 
252b 5h 39b 54b 
7^ T ‘ig T 
Ws 202 gh gh 
Q, = (19 - 42) 
DT; TQ T, Ty 
30? 8? 3? 2^? 
“ia pm P 
ag gh Zh TI, 


19.3 ”观测 模型 分 量 


假定 有 J 个 摄像 机 采集 刚体 状态 数据 ,观测 刚体 表面 所 标记 的 特征 点 。 每 个 
特征 点 相对 于 物体 重 ee 由 于 事先 校准 ,因此 公差 很 小 , 令 5 = 
Is; = [s si s ] ;i = 1, N) 是 表示 本 体 坐 标 系 中 特征 点 的 一 组 固定 的 三 
维 时 间 不 变 位 置 ， 因为 每 个 摄像 机 放置 位 置 不 同 ， 所 以 有 不 同 的 视角 ,每 个 摄像 
机 能 看 到 标记 的 不 同 子 集 。 对 于 c, 时 刻 的 第 j 个 摄像 机 ， a dapi 点 的 数目 
为 KK, ,将 摄像 机 的 每 个 可 见 特征 点 指定 为 hj, ,kj ，… n AHERN kja 
WES, eS HPL < kija SN, pace dob DEA AN. 摄像 
机 每 0. 005s 同时 记录 一 个 图 像 帧 ;所 有 摄像 机 的 位 置 和 方向 都 是 每 帧 校准 ,每 
个 摄像 机 帧 标记 图 像 与 标记 图 像 位 置 预测 的 关联 是 手动 管理 .图 19 - 3 给 出 的 
坐标 系统 描述 了 特征 点 投影 到 摄像 机 的 像 平面 的 原理 。 





摄像 机 坐标 系 中 的 
三 维特 征 点 (x.y,z) 


guo 
的 二 维 投影 (u,v) 


图 19 -3 特征 点 在 摄像 机 的 图 像 平面 上 的 投影 


J 
在 点 时 刻 的 观测 矢量 zx, HM, = > Ki, 个 二 维 像素 图 像 标 记 位 置 (u,v) 组 
成 ,z, 可 写成 
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Za = ENTF ET DS n 972, ,un 922,1.u,n 
T 
Zi jun *Ti j.e.n 5 ° E» „J.u n ZK), Jae d (19 - 43) 


目前 的 任务 是 定义 函数 2M， 维 观测 矢量 分 量 相对 于 状态 矢量 x, 的 相关 性 ,如果 
包括 观测 噪声 ,观测 值 可 以 根据 状态 矢量 写 为 


z, = h,(x,) + w, (19 — 44) 
h,(x,) = [b OR) pts. AR) LP MCN Pl, oio ] 
(19 — 45) 


这 样 剩 下 的 任务 就 是 定义 单个 子 函 数 。 
假设 观测 噪声 不 是 零 均 值 高 斯 过 程 , 为 w, 人 MO, R), FEP R EXER 
像素 对 角 噪 声 协 方差 窍 阵 的 测量 方差 ,可 表示 为 
R = o e. (19 - 46) 
这 里 Pw, 是 一 个 2M, x 2M, 的 单位 和 矩阵。 
Ez, h, 和 w, 都 是 2M, 维 ,x, 的 维 数 与 动态 模型 的 阶 数 相关 if M, Hl h, 
与 时 间 相 关 , 因 为 每 个 摄像 机 观测 到 特征 点 的 数目 是 随时 间 变 化 的 。 为 简单 起 
见 , 假 设 每 个 像素 分 量 噪声 方差 的 观测 值 er: 与 标记 特征 观测 值 相同 。 
附录 19.A 介 绍 了 点 旋转 和 帧 旋转 的 区 别 . 为 将 sk | 由 刚体 本 体 坐 标 系 经 过 
旋转 和 平移 ,变换 至 参考 坐标 系 ,应 进行 点 旋转 和 平移 , 即 
5. = Ms + p. (19 - 47) 
式 中 :p, At, 时 刻 位 置 矢量 的 质心 -旋转 矩阵 M。 可 以 通过 特征 标记 点 旋转 的 两 
个 步骤 来 得 。 第 一 步 ,将 旋转 a, 进行 轴 角 调整 , 与 表示 之 前 旋转 的 外 部 四 元 数 
q,., 相 结合 。 利 用 式 (19 - 164) 定义 的 函数 AA, 将 轴 角 矢量 a, 转换 为 旋转 矩阵 
M, , 即 


M, = M,(a,) (19 - 48) 
利用 式 (19 - 163) 定义 的 函数 M, ,将 外 部 四 元 数 g,, 变换 为 旋转 矩阵 M，， 
M, = ML) (19 -49) 
将 这 两 个 矩阵 相 乘 , 得 乘积 M.. 为 
M = M, M, , (19 - 50) 


将 st 进行 旋转 和 平移 ,由 参考 坐标 系 变换 至 第 /个 摄像 机 的 坐标 系 ,分 别 
将 摄像 机 C, 的 校准 位 置 .方向 和 焦距 定义 为 pc qos, 和 上 大 ,, 单 位 为 像素 ,然后 
进行 一 个 负 平移 ,之 后 是 帧 旋转 则 有 


wd s Me, Go - Pc.) (19 = $1) 
M. = A4 (qc) 
如 图 19 - 3 所 示 ,按照 下 式 将 三 维 si pn 
0 
sn) = (: Sjlo a Š e, (19 - 52) 
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sm 的 分 量 [ av。wyaiiws]7 e z, 可 写 为 


| Nh be bun (19 - 53) 
jl 


AP: (uw) 是 图 像 的 像素 坐标 ,以 像素 为 单位 。 方程 (19 - - 53) 表示 一 个 摄像 机 
第 i 个 特征 点 图 像 像 素 坐 标 位 置 ,因为 观测 矢量 由 M, 组 J 个 摄像 机 得 的 特征 点 
集 构成 ,观测 矢量 变 成 

| (19 - 54) 


其 中 Z; j.n = hi (x, ) 为 无 噪声 的 ,由 式 ( 19 - 47) E 式 ( 19 = 53) 给 出 。 


19.4 ”估计 方法 


可 以 用 一 些 估计 方法 来 进行 状态 矢量 和 协 方差 矩阵 的 递归 估计 。 下 一 节 将 
讨论 应 用 于 摄影 测量 法 跟踪 问题 的 三 种 方法 ,包括 NLLSQ 求解 器 高 斯 UKF 和 
无 迹 组 合 粒子 滤波 器 ,也 称 为 无 迹 高 斯 粒子 滤波 器 (UGPF) 。 本 节 给 出 这 三 种 估 
计 方 法 的 应 用 ,第 19.6 节 将 给 出 它们 的 性 能 比较 。 


19.4.1 非 线性 最 小 平方 估计 法 


以 前 基于 视频 图 像 数据 估计 刚体 的 位 置 和 方位 的 标准 方法 是 NLSSQ 方法 ， 
该 方法 假定 无 噪声 动态 方程 ,并 产生 实际 测量 集 和 预测 测量 集 之 间 的 最 小 均 方 
差 。 在 具体 实现 中 ,状态 矢量 中 只 有 刚体 在 c, 时 刻 的 姿势 。 因 为 在 每 个 时 间 步 需 
要 重新 估计 姿势 ,这 里 去 掉 所 有 时 间 下 标 。 
由 式 (19 — 31) ,将 NLSSQ 状态 矢量 定义 为 
x = [p',a']' (19 - 55) 
刚体 的 方向 由 a 和 表示 先 验 (或 初始 ) 方向 的 四 元 数 q 相 结合 进行 表示 。 
式 (19 - 54) 给 出 了 观测 矢量 。 观 测 矢量 与 状态 矢量 的 关系 可 以 写 为 
z=h(x)+w (19 - 56) 
其 中 h(x) 可 按照 式 (19 - 45) 分 解 为 h, (x), BRED h. (x) 由 式 (19 - ~ 


式 (19 - 53) 中 z 的 每 个 zv 分 量 给 出 -假定 w 人 N(OLR) ,其 中 有 M = x K, 


特征 标识 的 二 维 图 像 测量 值 ,R 为 2M x 2M 对 角 协 方差 矩阵 ， 其 中 对 所 有 的 图 像 
像素 位 置 采用 同样 的 方差 值 ,因此 可 以 有 
R = oll, (19 - 57) 
NLSSQ 方法 实质 上 是 对 给 定 观 测 值 z, 通 过 最 小 化 代价 函数 ,得 x 的 加 权 最 
小 二 乘 估计 , 即 
q(x) = [z - h(xz)]'R`'[z - h(x)] (19 - 58) 
因此 ,x 的 估计 值 x 可 以 由 下 式 得 , 即 
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Viq(x) =-2[ V,A (x) ]R'[z-h(x)] = 0 (19 - 59) 








Tt H ENH 
H å V,h'(x) = V [Ba o) Po o) ,hx (Xx) ] 
ðh, | dh, | m Ohx, 
9p, — Op, ðP, 
dh,, óh, — Óh,,, 
=| ðP, Op, ap, (19 - 60) 
9h,, dh, WT hy, 
da, da, da, 


非 线 性 函数 h, ,将 x 变换 为 第 J 个 摄像 机 图 像 中 第 i 个 标记 点 ,进而 推导 出 z,,。 这 
样式 (19 - 59) 可 重 写 为 
HR`'[z -h(x)] =0 (19 — 61) 
给 定 状态 的 某 些 初始 估计 (推测 )x, ,将 非 线性 函数 h(x) 展开 为 关于 x, 的 
泰勒 展开 式 , 即 


h(x) = h(xo) * [ V,h'(x)]r.s[x 2x] +… (19 - 62) 
或 者 , 仅 保持 展开 式 的 线性 项 
h(x) = h(x,) + H,[x - x. ] (19 - 63) 
H, À [ V ,h'(x)];-;, (19 - 64) 
方程 (19 - 61) 变 为 
HR^"[z - h(x,) - H,[x - x, ]] = 0 (19 - 65) 
& HH, RX, = x 得 
x, =x, +[H,R HT] HR" [z - h(x,)] (19 - 66) 


使 用 x BS THE DU A hh. A ik M EERE E p 次 迭代 后 ,得 Gauss - 
Newton 标准 方程 , 即 


x, =x, +[H, RH ] H, R [z -h(x,)] (19 -67) 
x, 的 协 方差 为 


F = [R .| (19 - 68) 

由 式 (19 — 57) 可 知 R HITHER ,那么 上 面 两 个 方程 可 简化 为 
x, =x, + (H!) [z - k(x,.,)] (19 - 69) 
P, = ol H, Hi, ] (19 - 70) 


重复 上 述 过 程 ,直到 e, à x, -r,a KAD FA EN ARE. 
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因此 ,在 每 个 ,时间 步 ,可 得 到 一 组 新 的 图 像 z, 。 重 复 这 一 计算 过 程 ,最 终生 
成 估计 值 *，= x,,, ,其 估计 协 方差 为 P,, ,注意 ,在 Matlab 中 , 子 程序 “lsqnonlin” 


就 是 NLSSQ 方 法 ,输入 为 函数 (19 - 58) 和 变量 x, 输 出 为 使 函数 最 小 化 的 x 值 。 
尽管 这 个 函数 不 估计 协 方差 矩阵 ,但 是 某 些 形式 的 函数 会 返回 雅 可 比 矩 阵 互 ，， 
用 来 计算 协 方差 矩阵 估计 值 。 

因为 NLSSQ 公式 是 迭代 但 不 递归 的 (在 时 间 上 ) ,没有 用 到 动态 模型 ,观测 
值 只 与 刚体 的 位 置 和 方向 相关 ,只 能 估计 位 置 和 方向 ,为 了 估计 任意 位 置 和 方向 
的 时 间 导 数 ,必须 用 某 种 形式 的 平滑 滤波 器 或 有 限 差分 滤波 器 作为 后 处 理 器 ,来 
得 x 的 时 间 导 数 。 

作为 进一步 的 研究 方向 ,可 以 考虑 在 矢量 h(x) 中 包括 一 个 动态 模型 或 者 
加 入 时 间 上 连续 的 观测 值 对 其 进行 参数 化 -这 将 允许 状态 矢量 中 包含 导数 项 , 估 
计 过 程 也 可 以 包含 导数 项 ,但 这 将 增加 h(x) 和 互 的 维 数 ,进而 增加 计算 量 。 


19.4.2 无 迹 卡 尔 曼 滤波 器 法 


UKF 是 Sigma 点 卡尔 曼 滤波 器 的 一 种 , 它 要 求 所 有 的 噪声 密度 都 是 高 斯 分 
布 的 ,对 于 一 个 i 阶 模型 ,其 动态 方程 为 非 线 性 的 ,由 式 (19 -33) 给 出 ;观测 模型 
也 是 非 线性 的 ,如 式 (19 - 44) 所 示 。 假 定 动态 噪声 密度 和 观测 噪声 密度 均 为 高 
斯 分 布 ,根据 本 书 第 H 部 分 内 容 可 知 , 如 果 对 非 线性 函数 进行 仿 射 变 换 , 然 后 将 
它们 展开 为 一 般 多 项 式 , 高 斯 加 权 和 矩 积分 可 简化 为 Sigma 点 集 的 加 权 和 ,这 里 权 
值 和 Sigma 点 取 值 由 所 选 的 Sigma 点 滤波 器 决定 .本 节 提出 用 一 般 Sigma 点 卡尔 
曼 滤 波 器 公式 来 估计 状态 矢量 ,其 协 方差 矩阵 基于 时 间 连 续 图 像 特征 标记 观测 
值 集合 。 

滤波 器 首先 进行 状态 矢量 x, = [Poa] 的 初始 估计 ,得 相关 的 四 元 数 
go( 详 见 第 19.4.4.2 节 ) 及 其 协 方差 PF 。 根 据 第 19. 2. 1 节 和 第 19.2.4 节 ,x 的 维 


BE P= 和 Q@ 值 均 取决 于 所 选 的 模型 。 
状态 Sigma 点 计算 公式 为 
Rosina = S ca * D, i. e" J = 0,1 y s= WN, (19 e 71) 
这 里 E ai — X 和 P ud 一 Po 为 Sigma 点 的 初始 集合 ,D, uua 定义 为 
Print = D, w. 4D usi (19 - 72) 


式 中 ;NN, 为 Sigma 点 的 数目 ;c” 7928 j Sigma 点 。Sigma 点 滤波 器 中 用 到 的 集合 
le, e? | 取决 于 状态 矢量 的 大 小 和 所 要 实现 的 Sigma 点 滤波 器 , 可 以 使 用 列 
表 13 - 1 和 表 13 -2 中 给 出 的 子 程序 来 生成 。 
根据 第 13 章 内 容 ,Sigma 点 卡尔 曼 滤 波 器 的 状态 预测 方程 为 
X2 1 = fad (19 = 73) 
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EN = Y 90; No et (19 - 74) 
P = Y va. = Et) ha Suet)" +Q (19 -75) 
观测 预测 方程 为 
£47 Y hr) (19 - 76) 
Per = Dw BOs) -2u AAR) ~ Ean]? eR 
(19 - 77) 


Pa = Yuly? -ual ARLA) -Zr (19-78) 
状态 更 新 方程 为 


K, ATL (OS 07 (19 - 79) 
FA IMP + K, (z? = Zani) (19 — 80) 
PS. = Pia ~ K,P3, UE (19 -81) 


sth iz? H t, 时刻 的 观测 值 ,一 旦 在 滤波 器 迭代 最 后 得 状态 矢量 xu, = [pL,, 
a’ )" Ha, #lq,., 生成 新 的 单位 四 元 数 gq, ,之 后 根据 下 式 重 置 4,, , 即 

qn = Qalanin) aai (19 EX 82) 

à, , — [0,0,0]* (19 — 83) 

对 于 高 阶 模型 和 大 量 特征 标记 图 像 尺 寸 ,UKF 是 最 好 的 Sigma 点 卡尔 曼 滤 

波 器 ,因为 所 需 Sigma 点 的 数目 为 N= 2n +1, 这 里 n 为 状态 矢量 的 维 数 ,在 UKF 

滤波 器 的 所 有 应 用 中 ,通常 选择 w。= 0, 因 此 N, =2n Ho, 2 172n,j = 1,…,2n。 

此 外 ,em = Jnr” = Vn[1] e R"( 请 参阅 第 2.2 节 的 符号 解释 ) 。 球 面 单 形 卡尔 

曼 滤 波 器 (spherical simplex Kalman filter, SSKF) H AE (N, = n+1) ,简单 检 

验 其 性 能 发 现 , 其 性 能 结果 与 UKF 相似 ,这 里 不 详细 讨论 。 


19.4.3 无 迹 组 合 粒 子 滤波 器 估计 法 


第 17 章 介绍 了 组 合 粒 子 滤波 器 , 即 无 需 进 行 重 采样 的 粒子 滤波 方法 。 这 里 
用 它 来 替代 NLSSQ。 组 合 粒子 滤波 器 的 过 程 框图 中 利用 UKF 来 生成 图 17 - 3 中 
的 重要 性 采样 ,在 将 组 合 粒子 滤波 器 应 用 到 摄影 测量 法 刚体 姿态 估计 问题 时 ,每 
个 粒子 的 似 然 函 数 p(z, | x; ) 为 高 斯 的 , 式 (19 - 44) 可 改写 为 
W = z, -h(x ) (19 - 84) 
RA plz, l x) = Miz -h(x ) ] R) ,可 以 将 粒子 似 然 函数 写 为 
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p(z, | 5 = cexp{ - ium -h (x? ) J'R^ [2° _ h, (xi?) }} 


(19 - 85) 
除非 是 数值 稳定 性 需要 ,不 需要 包括 规范 化 的 ,因为 在 权 值 更 新 步骤 的 权 值 归 
一 化 过 程 中 会 删除 这 个 变量 。 在 这 个 应 用 中 ,使 用 20000 个 粒子 .图 17 -3 中 已 给 
出 整个 步骤 ,这 里 不 再 袭 述 。 


19.4.4 ”估计 器 初始 化 
上 述 所 有 的 估计 方法 都 需要 一 个 初始 估计 x, 有 一 些 还 需要 PU 的 初始 估 


计 。 然 而 ,NLLSQ 估计 器 和 所 有 其 他 估计 器 在 初始 估计 所 代表 的 意义 上 存在 一 
些 差异 .下 面 各 小 节 将 研究 这 些 问 题 。 


19.4.4.1 NLLSQ 初始 化 


由 于 NLLSQ 是 一 个 和 迭代 滤波 器 (在 一 组 代码 中 循环 ,直到 达到 容许 误差 的 
限制 ) ,在 每 个 时 间 步 骤 中 必须 重新 初始 化 。 在 投放 事件 发 生 之 前 的 时 间 步 中 ， 
刚体 是 静止 的 ,所 以 初始 估计 的 位 置 和 方向 可 设 为 


x, = [pT,aT]7 = [0,0,0,0,0,0]' (19 - 86) 
这 样 ,qo 的 初始 估计 为 一 个 四 元 数 ,表示 零 角 度 , 即 
q = (1,0,0,0) (19 - 87) 


对 于 其 它 不 同 于 模拟 测试 的 情况 ,p; 和 go 额定 值 可 以 设 为 非 零 值 ,而 a, 应 始终 
设置 为 ad = [0,0,0]"。 

在 投放 事件 之 后 ,刚体 在 一 个 时 间 步 到 下 一 个 之 间 进 行 平移 和 旋转 ,所 以 从 
上 一 时 间 步 估计 得 的 位 置 和 旋转 的 最 终 估 计 可 以 用 来 初始 化 当前 时 间 步 的 状态 


矢量 。 设 x+ = [pi,aiT]T 为 第 m” 帧 的 后 验 估 计 ,4,, 表示 从 第 n - 1 的 整个 旋转 ， 
那么 9, Fa, 可 设置 为 


q, = (19 - 88) 
a, = [0,0,0]7 (19 - 89) 
第 ”+ 1 帧 的 先 验 初始 估计 可 设 为 x;,，= [pila )' A 
Pai sR (19 - 90) 
a~,, =a} = [0,0,0]7 (19 -91) 


19.4.4.2 贝 叶 斯 滤波 器 初始 化 


对 于 除 NLLSQ 外 的 所 有 滤波 器 ,需要 设置 PP 和 gu 的 初始 值 ,为 保持 一 
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致 ,这 里 对 所 有 的 滤波 器 使 用 相同 的 初始 值 ,但 是 ,不 同 的 动态 模型 ,所 需 的 初始 
值 也 不 相同 ,所 以 ,对 于 不 同 阶 数 的 模型 ,需要 不 同 的 初始 值 :恒定 位 置 /方向 ， 
恒定 速度 , 恒 加 速度 以 及 恒 加 加 速度 。 因 为 可 以 把 低 阶 模型 当做 其 邻近 高 阶 模型 
的 退化 ,因此 仅 需 初始 化 最 高 阶 (三 阶 ) 的 模型 变量 x PO 和 go ,在 需要 初始 


化 低 阶 模型 时 将 其 截断 ,因为 刚体 初始 为 静止 ,xs 的 初始 估计 为 
xo) - |: - 0 (19 - 92) 


式 中 :0 为 包含 24 个 零 的 矢量 。 
模拟 测试 中 gu。 的 初始 估计 为 单位 四 元 数 ,表示 角度 为 零 , 即 
Q, = (1,0,0,0) (19 - 93) 
在 模拟 测试 以 外 的 其 它 情况 中 ,p。 和 g FLARES ME fH a 仍 设置 为 G, = 
[0,0,0]"。 因 为 通常 物体 都 是 静止 的 ,p,。 和 au 的 导数 可 仍 设置 为 零 。 
为 初始 化 PF A o, o; o; AM o; 分 别 为 平移 位 置 速度、 加 速度 和 加 加 速 
度 的 标准 偏差 ,0, o; o; 和 zz 分 别 为 角度 位 置 速度 、 加 速度 和 加 加 速度 的 标 
准 偏差 ,将 标准 偏差 设 为 
a, =1 o;=5 c; =100 c; = 5000 (19 — 94) 
Ns 1 0;=1 og; =50 gy = 2000 (19 - 95) 
其 中 平移 的 单位 是 in 和 s; 角 度 的 单位 是 deg 和 s。 这 样 ,平移 协 方差 子 阵 的 初始 
值 为 


ol, 0, 0, 9%, 
pro l| ih os ks (19 - 96) 
0, 0, os 1, 0, 
0, 0, 0, o; 1, 
角度 协 方差 子 阵 的 初始 值 为 
o oS a 0, 
0, ol, 0 0 
MO c ` T < I (19 - 97) 


0, 0, c.l, 0, 

0, 0, 0 oil, 

其 中 Cy, = «/180 将 度 变换 为 弧度 。 结 合 这 两 个 子 和 矩阵 ,构成 最 初 的 协 方 差 矩 
阵 , 即 


Pa(3) 0 
(3) ¿ O.trans 12 
P, - p »: (19 — 98) 
12 1 0 ,ang 
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在 滤波 过 程 开始 之 后 ,很 少 重新 初始 化 贝 叶 斯 估计 器 ,因此 仅 需 进行 一 次 上 
述 初 始 化 过 程 。 


19.5 生成 模拟 数据 


为 评估 各 种 跟踪 滤波 器 方法 的 性 能 ,提出 了 一 种 模拟 刚体 ,并 生成 弹药 投放 
事件 的 模拟 轨迹 。 这 提供 了 一 个 模拟 的 “真实 ”轨迹 ,来 创建 模拟 观测 集 ,例如 可 
采用 模拟 摄像 机 通过 添加 蒙特 卡 洛 集合 像素 噪声 来 得 到 。 模 拟 的 摄像 机 测量 值 
可 以 用 于 估计 (求解 ) 方法 蒙特 卡 罗 均 方 根 误差 分 析 比 较 。 


19. 5.1 模拟 刚体 特征 点 


用 单位 立方 体 构造 一 个 有 8 个 特征 点 的 刚体 ,构造 一 个 3 x 8 的 矩阵 $ ,这 里 
S 的 第 i 列 为 特征 点 $; = EPR sS; y "- , 即 


-1 1 -1 1 -1 1 -1 1 
HE -1 1 1 -1 -1 1 ] (19 - 99) 

-1 -1 -1 -1 1 l 1 1 
运用 一 个 仿 射 变换 ,使 立方 体 变 得 细 长 , 即 


S = TS (19 - 100) 
200 0 0 

r =| o 30 o (19 - 101) 
0 0 30 


19.5.2 模拟 轨迹 


为 建立 一 个 模拟 轨迹 ,生成 一 个 时 间 t 的 序列 , 即 
t = |t, 2 0.005*n,n =0,1,2,.…,200| (19 - 102) 


定义 一 个 阻尼 正弦 函数 (阻尼 谐振 荡 器 ) , 即 
K t, ,a,B,y) = wr | cos[ y(t, z t,)] - 1 t, > t, 
0 & < z. 
(19 - 103) 
式 中 :t, 为 物体 投放 时 间 ( 设 1, = 0.1) ;常数 ac 和 ?7 分别 为 振幅 阻尼 系数 和 频 
RH t, 时 刻 之 前 物体 没有 运动 ,而 在 t, 时 刻 之 后 物体 下 落 。 
f(t,,a,B,Y) 的 前 两 阶 导数 为 
- qe P | Beos[ y(t, -1,)] - 
f(t, ,0,B,y) = | + ysin[ y(t, —t,) ]] 
0 Lo A 
(19 - 104) 
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Ë 35 4, 


- ae ^^"? | (g? -y )eos[y(t, -1,)] - 


i L s£ 
f(t, ,@,8,y) = (B + 2Bysin[ y(t, - ¢,) ] | 
0 < k 
(19 - 105) 


利用 阻尼 正弦 信号 生成 6 P FF 9, BED ATI (x,7,2,6,0,0) 中 六 个 参数 中 

的 一 个 ,其 中 由 2 和 yw 为 翻滚 俯仰 和 偏转 的 欧 拉 角 , 且 t, e (GA 
x(t,) = f(t,,a,,B,,y,.) (a,,B,.y.) = (100,2,2) (19 - 106) 
y(t,) = f(t,,a,,B,,y,) (a,,B,.y,) = (= 200,3,1) (19 - 107) 

z(t,) = f(t,,0,,B., y.) (a,,B,, y.) = (- 2000,1.5,0.5) 


(19 - 108) 
b(t,) = f(t,,a,,B,.y,) (a,.,B, v.) = (500,1.5,1.25) 
(19 - 109) 


O(t,) = f(t,,m, BoY) (osos y,) = (-10,2,10) (19 - 110) 
e(t) = ftn Ap Be Ye) (a,.B,.y,) = (15,0.8,14) (19 - 111) 
然后 根据 式 (19 - 104) 和 式 (19 - 105) Æ xCt) y(t,) a) C) 9) . 
eG) VAR x(t,) JG.) .2(t,) e) 0) P(t, ) o 
可 以 很 容易 地 构造 模拟 轨迹 的 平移 分 量 , 即 


Pose = [z(z,),y(t.) ,z(#,) ]" (19 - 112) 
Poss = [z(t.) YCE) ,2(8,.) )” (19 - 113) 
Paine = [%(8,) yC) C)" (19 - 114) 
对 于 旋转 分 量 ,必须 构造 轴 角 a, su FUR (E OTC qn irse RTF a,n ,有 
a,,, = [0,0,0]', Vn (19 - 115) 


为 构造 q, ,首先 引入 转换 函数 @eue( 由 ,6,p) :R — HEEL ff6,0,9) 转 
换 为 单位 四 元 数 , 即 


Qi (6.0,9) = 94909. (19 - 116) 
这 里 q,.q, fü q, 是 四 元 数 ,代表 三 个 欧 拉 角 , 即 
q, = [cos sin ,0,0] (19 - 117) 
q, = [cos &,0,sin 9,0] (19 - 118) 
q, = [ cos £,0,0,sin £ j (19 - 119) 


q... . 可 定义 为 
Cas uno = Quas Ct, ) ,0( t.) ot.) ) 


= 95909. (19 - 120) 
272 


为 简洁 起 见 ,省 略 了 ”nm,true” 下 标 部 分 , 式 (19 - 15) 的 四 元 数 乘法 得 到 了 间接 
表述 。 设 

Bona LD am) = Lp kT (19 - 121) 

为 构造 真实 数据 来 比较 a, Ma, ,必须 给 出 模拟 的 旋转 一 阶 导 数 和 二 阶 导数 

表达 式 , 由 于 使 用 本 体 参 考 坐 标 系 进行 求 导 更 为 直观 ,所 以 这 里 在 本 体 参 考 坐 标 

而 不 是 固定 参考 帧 中 进行 求 导 运算 ,将 本 体 参 考 角 度 导数 记 为 awe” 和 a。 
回顾 著名 的 四 元 数 微分 公式 所 BB 

q = jeg (19 - 122) 


这 里 q 为 单位 四 元 数 ,四 元 数 w = [0,w, ,w, ,w,]" 可 将 本 体 参考 旋转 速率 矢量 转 
换 为 纯 四 元 数 (因为 其 标量 分 量 为 0, 因 此 称 其 为 " 纯 ”) 。 帧 旋转 中 9.4 和 ww 之 间 的 
关系 由 式 (19 - 122) 指定 ,而 q, ne 为 点 旋转 。 如 果 4 为 帧 旋转 , 则 q.s. 等 同 于 点 
旋转 ,那么 gq, ime = 4 ”, 这 里 4 ”为 4 的 共 配 四 元 数 . 即 ,如 果 qg = [9,,9..4,.9.] . 
那么 qg”= [g,, -q., —q,, -9:] -回顾 四 元 数 代数 的 三 个 事实 ,对 式 (19 - 124) 
WB Pky, Bp : CD 如 果 w 是 纯 四 元 数 (其 中 标量 分 量 w, = 0) ,那么 w* = - w; 
@ 对 于 四 元 数 g, 和 g, 的 乘积 ,有 (gg:) ”= q; q; ;@ MPTP UIE , 48 
原 四 元 数 , 即 (9 ) ”= 4。 在 式 (19 - 122) 中 ,用 9 替换 g . 并 用 w FM 


W, ime ,得 
Qipa => z PY Dum (19 = 123 ) 
求解 eo, ,得 


Drive = — 2q7 ire (Anine) 
(En ime) ` 
lI g..—= | 
m. v NEM (19 - 124) 
其 中 用 到 了 qni 为 单位 四 元 数 这 一 事实 。 
对 式 (19 - 124) HAAA y , 14 
Oe true = — @,— = —2(Q edo) 7724, q... (19 ~ 125) 


a= 2q; true 


所 以 有 
€, true = r ail x. (19 一 126) 


为 计算 eo, ,必须 先 求 得 9，。。 为 此 ,对 式 (19 - 120) 用 乘法 规则 求 微分 ， 
Bl 
"E ACREA, 


= 9909s + q,q,q, + q.q,q, (19 - 127) 
273 


这 里 q, .q, fl q, 可 通过 对 式 (19 - 117) ~ 式 (19 - 119) 取 微 分 来 得 , 即 


，_ 由 6 baal 
q, = 2 | — sin Arcos 50,0] (19 — 128) 
: ⁄ T 
q, = £| — sin £ ,0 ,cos 2 .0| (19 — 129) 
a — @ N 
4 7 >| -和 分 ,0,0,eos ej (19 - 130) 
将 式 (19 - 127) 代入 式 (19 - 126) ,得 
O, ee = 2q;.. (4,050, + Aolle + q,q,q,) (19 - 131) 
RE alt? 可 写成 
n ipn = ee Dy 5 true P NEN 1 (19 us 132) 


用 类 似 的 方法 ,为 生成 di 必须 求 得 wwe。 通过 对 式 (19 - 126) 进行 微 
分 计算 , 即 





dg... dq d'q 
一 2 n.true n, true * n, true ] = 1 3 
[m mre (19 - 133) 


为 计算 dq; /dt, 观 察 到 对 于 所 有 的 gqg, 有 d(qq”* )/dt = 0, 因 此 











d. dq ` . dq 

mL rma ba 3 ( ) 
进而 得 

d d . p. B 

4 — “Šg --4'qq (19 — 135) 

dt dt 


将 式 (19 - 135) 代入 式 (19 - 133) ,得 
[= 
= 29; c usa Mu (19 - 136) 
对 式 (19 - 127) 取 微 分 ,得 q, true 为 
q... = Q.Q,q, + q,Q.q, + q.q,q, + 


20.4,4,, + 29.9094 + 20.4,4, (19 = 137) 
通过 对 式 (19 - 128) ~ 式 (19 - 130) 取 微分 ,得 ggo, 和 89。 为 
T 
q, = $[- a $ „cos £,0,0] - og, (19 — 138) 
NN 6 : e e T cj 7 
q, = 7 | - sin 7 ,0 ,cos 2 ,0| 0q, (19 - 139) 
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ME r. 
q, = S|- sin £.0,0, cos $] - eq, (19 - 140) 


将 式 (19 - 137) 代入 式 (19 - 136) ,得 
Dy rove = 24, (q.q,q, + q.q,q, + 4,d,0, + 2q,q,q, + 


2q,q,q, + 29,9094) — nne] (19 - 141) 
Hija, 可 写成 
ipe = C= i s s. ae le (19 xd 142) 


19.5.3 模拟 摄像 机 


下 面 生成 模拟 数据 ,假定 只 有 两 个 模拟 摄像 机 ,设置 /= 2, 并 为 它们 指定 位 
置 方向 (单位 为 ") 和 焦距 (单位 为 像素 ) , 即 


(xc ,yc ,zc ) = (490,100,0) (19 - 143) 
(xc,,Xc,,ze,) = (-490,100,0) (19 - 144) 
(de, ,bc ,pc Je.) = (0,0, - 168,2500) (19 - 145) 
(dc, ,bc Pc, Se) = (0,0, - 12,2500) (19 - 146) 
然后 设置 pc, dc, Pc, dc, 为 
Po, = [teca] (19 - 147) 
qc, = Qiuer( Poe, ,gc Pc, ) (19 - 148) 
Po, = [xo Yo Za ] (19 - 149) 
qc, = Qku.( Óbc,,0c,,@c,) (19 - 150) 


19.5.4 模拟 测量 


用 式 (19 - 100) 构造 模拟 刚体 之 后 ,对 于 所 有 的 iv 和 nn, 通 过 设置 人 ,和 
k, j,n age 
ne iay ={1,2},n = {0,1,---,200} 有 
K, = 8; 而 对 于 i = 11,2,,8] j = {1,2},n = {0,1,- 2801 We, = i 
19. 3 节 已 经 给 出 构造 测量 函数 h(x) 所 必须 的 每 一 项 的 表达 式 , 然 后 用 h(x) 去 
生成 模拟 测量 值 .对 于 每 个 ” = 10,1,…,2001 ,通过 计算 生成 测量 矢量 z。 synir BI 


Coens = B Xu) + W, uk (19 - 151) 
W, synth ^ MO, Rea) (19 = 152) 
Ra synth = 0 alg, - 6 I (19 - 153) 


IX M, = 16 = Y K. sla 为 一 个 32 x 32 单位 矩阵 ,而 o, 设 为 2 个 像素 。 注 


E, 每 个 时 间 步 都 要 从 AO Rous) 抽取 新 的 蒙特 卡 洛 采 样 。 
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19.6 ”性 能 对 比分 析 


对 第 19.4 节 中 给 出 的 所 有 估计 滤波 器 ,用 第 19. 5 节 描 述 的 模拟 测试 数据 进 
行 试验 ,以 进行 性 能 分 析 。 使 用 零 阶 一 阶 ,二 阶 和 三 阶 动态 模 型 UKF ,与 NLLSQ 
进行 性 能 比较 ,并 与 使 用 二 阶 动态 模型 的 UGP 进行 比较 .所 有 情况 下 使 用 的 性 
能 指标 都 是 均 方 根 (RMS) 误差 下 面 给 出 的 性 能 结果 表明 ,二 阶 UKF 是 最 好 的 
滤波 器 。 

图 19 -4 至 图 19 -6 给 出 二 阶 模型 UKF 滤波 器 单 次 运行 的 估计 轨迹 与 模拟 
真实 数据 比较 的 结果 。 观 察 到 图 19 -5 和 图 19 -6 中 的 旋转 速率 和 加 速度 都 是 在 
本 体 坐 标 系 中 的 。 图 19 -7 给 出 了 横向 位 置 真实 航 迹 一 小 部 分 特写 ,由 多 个 求解 
器 得 的 估计 航 迹 全 加 在 真实 航 迹 上 。 所 有 估计 算法 用 相同 的 含 品 观 测 值 和 初始 
值 作为 输入 。 通 过 数据 分 析 , 得 出 两 个 结论 :(D NLLSQ 和 零 阶 动力 学 模型 UKF 
的 估计 结果 几乎 相同 。 因 为 零 阶 模型 是 一 个 恒定 位 置 模型 ,而 NLLSQ 假定 为 只 
有 位 置 的 状态 矢量 ,可 以 预料 到 NLLSQ A UKF EK) 在 性 能 上 的 相似 性 -人 @) 使 
用 高 阶 动态 模型 的 滤波 器 性 能 优 于 NLLSQ 或 使 用 零 阶 模型 的 滤波 器 , 主要 是 由 
于 减少 了 估计 值 相 对 于 真实 航 迹 的 变化 .第 19. 5. 2 节 给 出 的 各 种 滤波 器 和 模型 
的 均 方 根 误差 比较 得 出 这 一 初步 的 结论 。 


平移 位 置 /in 
平移 位 置 /in 






| 
0 02 04 06 08 


od is 
时 间 /s 时 间 /s 
(a ) X ff] Wi (b) YAA 
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旋转 位 置 /( ) 
N "x2 ROS œ 





“0 02 04 06 08 |! 
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0 02 04 06 08 1 
时 间 /s 
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图 19 -4 用 二 阶 模型 UKF 滤波 器 进行 平移 和 旋转 的 位 置 
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用 二 阶 模型 UKF 滤波 器 进行 平移 和 旋转 的 速度 
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图 19 -6 用 二 阶 模型 UKF 滤波 器 的 平移 和 旋转 加 速度 
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横向 位 置 /in 





0.5 0.55 0.6 0.65 0.7 0.75 0.8 


图 19 -7 用 多 个 求解 器 使 用 相同 的 输入 进行 横向 位 置 估计 


19.6.1 滤波 器 性 能 对 比方 法 


性 能 比较 方法 用 第 19. 5.2 节 的 方法 生成 一 个 无 噪声 的 状态 矢量 航 迹 |x, ,n = 
0,1,…,N|} ;然后 将 这 个 航 迹 转换 为 一 组 无 噪声 的 模拟 真实 观测 值 |h, (x) ,n = 
0,1,…,N| ;最 后 ,生成 Neus = 100 组 含 品 观测 值 ,通过 向 模拟 真实 观测 值 中 添 
加 独立 零 均 值 高 斯 噪声 ,生成 含 噪 观测 值 的 Naws EA ze = 0,1,…,Nir = 
1 ,2 ,… News | 。 在 每 个 跟踪 滤波 器 中 ,用 每 个 含 品 观测 值 集 合 来 产生 状态 矢量 估 
计 的 Neus 集合 |zr in = 0,1,…,Nir = 1,2,…,Naws|。 

第 14 章 指出 ,在 六 时 刻 完 整 矢量 估计 xus 的 协 方差 矩阵 可 以 写 为 

PZ. = Ella, -Z,.)(x, = Ra) (19 - 154) 


M x, 为 已 知 ,因为 是 模拟 航 迹 的 情况 ,而 zu, 是 x, 的 无 偏 估计 ,对 角 项 的 方 根 
PY, 可 以 通过 重复 滤波 器 运行 来 估计 ,对 每 次 运行 使 用 不 同 的 蒙特 卡 洛 集合 含 
品 观 测 数据 来 得 条 件 均 方 根 误差 。 

例如 ,滤波 器 在 i, 时 刻 的 横向 位 置 (y) 的 RMS 误差 为 


2 





NRMS ^ ( filter) ) 2 
I 


rag Vn Yatay 
N rms 
比较 6 维 问题 每 个 滤波 器 所 得 的 零 阶 、 一 阶 、 二 阶 项 结果 ,需要 计算 并 分 析 
18 个 参数 的 RMS 误差 .为 简化 分 析 , 使 用 距离 的 平方 ,同时 比较 位 置 的 3 个 分 
量 , 即 
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(19 - 155) 


(filter) _ 
Cyn im 


A 


at - 2 l 
|p. - pass dl 0-2) + (y, — asr > 


^ 


" (z, = on ys 
(19 - 156) 
这 个 结果 只 有 6 个 性 能 比较 ,分 别 是 每 个 3D 位 置 及 其 前 两 阶 导数 ,还 有 每 个 3D 
旋转 及 其 前 两 阶 导 数 。 
3 个 平移 参数 包括 t, 时 刻 的 位 置 ,RMS 位 置 误差 计算 公式 为 


(her) _ Y oy | Pa -P 
" ü Nems 
对 于 位 置 的 一 阶 和 二 阶 导数 ,用 同样 方式 计算 — 和 es o 
因为 3D 角度 不 是 矢量 ,所 以 不 能 用 同样 的 方式 计算 角 位 移 的 均 方 根 误差 。 
用 单位 四 元 数 表示 两 个 方向 q, 和 gq, 之 间 的 差异 ,可 以 有 
q, = % 4 (19 - 158) 


为 计算 差 值 ,必须 将 qs 转换 为 轴 角 表达 式 ,之 后 才能 计算 它 的 范 数 。 因 此 ,可 以 
将 某 一 具体 滤波 器 在 t, 时 刻 的 RMS 角 误 差 写 成 





(filter) 2 
nln,r | 


(19 - 157) 


NEMS fil 2 
h N aa MALCE Gans || 
NRws 





(filter) _ 


(19 - 159) 
这 里 A,:H 一 R (人 参见 式 (19 - 163) ) 将 单位 四 元 数 转换 为 一 个 轴 角 矢量 , 而 
Qe) 为 表示 特定 滤波 器 在 i, 时 刻 第 r 次 运行 的 方向 估计 的 四 元 数 ,q, 表示 1, 时 
刻 的 真实 方向 。 

因为 旋转 速率 a 是 一 个 矢量 ,可 以 直接 比较 aw” Fla, ,滤波 器 估 a 计 是 在 
参考 帧 坐标 系 ,而 & 的 真实 航 迹 是 在 本 体 参 考 坐 标 系 ,因此 ,必须 用 帧 旋转 (参见 
附录 19. A. 3) BEG‘? 转换 至 本 体 坐 标 系 ,这 样 ,特定 滤波 器 在 i 时刻 的 RMS 角 
速率 误差 可 写 为 











avr 


Neus 
这 里 M,，= M, (qin) EAI, RRA PE EK REE t, WTP r USAT AH 
itr ty et 可 以 用 同样 的 方式 进行 计算 。 

19.6.2 滤波 器 对 比 结果 


对 于 每 个 滤波 器 和 模型 阶 数 ,将 RMS 误差 作为 时 间 的 函数 来 进行 计算 , 然 
后 对 每 种 情况 生成 均值 和 最 大 误差 (一 段 时 间 内 )。 表 19 -1、 表 19 -2 和 表 19 -3 
分 别 概括 了 位 置 .速度 和 加 速度 RMS 误差 的 均值 和 最 大 值 。 


NRMS T (fil "oy? 
Y "* MT at — a, | 





(filler) _ 
Cain = 


(19 - 160) 


279 


表 19 -1 模拟 数据 RMS 位 置 误差 
| 


NLLSQ 


UKF 
UKF 
UKF 





19-2 模拟 数据 RMS 速度 误差 


平移 速度 7 ( ft/s) fa HE /((°)/s) 


BK 均值 
0 N/A 
0 
l 
2 
3 
2 


表 19 -3 模拟 数据 RMS 加 速度 误差 


平移 加 速度 /g ffi IMR HE /( (° )/s*) 
滤波 器 类 型 模型 阶 数 
N/A N/A N/A N/A 








图 19 -8、 图 19 -9 和 图 19 - 10 分 别 给 出 每 种 滤波 器 /模型 次 序 组 合 的 时 
程 : 位 置 和 旋转 RMS 误差 、 位 置 和 旋转 速度 RMS 误差 以 及 位 置 和 旋转 加 速度 
RMS。 由 表 19 -1 ~ 表 19 -3 和 图 19 -8 ~ 图 19 -10 明 显 可 见 ,所 有 滤波 器 中 
使 用 的 模型 阶 数 都 大 于 零 ,其 平均 性 能 在 位 置 上 至 少 提 高 50% ,在 方向 上 提高 
30% -因为 物体 在 0. 1s 时 投放 ,加 速度 的 变化 非常 大 ,RMS 误差 曲线 表明 在 投放 
事件 后 200ms ~ 300ms 时 性 能 最 差 。 

二 阶 无 迹 高 斯 粒子 滤波 器 (UGPF(2)) 和 二 阶 无 迹 卡 尔 曼 滤波 器 
(UKF(2)) 在 6 个 参数 类 型 上 的 性 能 几乎 相同 ,因为 用 高 斯 随机 误差 进行 模拟 
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位 置 RMS 误差 /in 





0 01 02 03 04 0.5 0.6 07 08 0.9 I 
Bef l] /s 





0 0.1 0.2 03 0.4 0.5 0.6 0.7 0.8 0.9 l 
时 间 /s 


+- NLLSQ——UKF (0) ° UKF (1) -一 UKF (2) —UKEF (3) »* UGPF (2) 





图 19 -8 所 有 跟踪 滤波 器 使 用 模拟 数据 得 的 位 置 和 方向 RMS 误差 


位 置 速度 RMS 误差 /(fUs) 





If fr] /s 


方向 速度 RMS 13€ (C ys) 





° UKF (1) —— UKF (2) ——UKF (3) -*- UGPF (2) 





图 19 -9 所 有 跟踪 滤波 器 使 用 模拟 数据 得 的 位 置 和 方向 速度 的 RMS 误差 
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时 间 /s 
——UKF (2) —-UKF (3) ^ *- UGPF (2) 


图 19 —10 所 有 跟踪 滤波 器 使 用 模拟 数据 得 的 位 置 和 方向 加 速度 的 RMS 误差 


测量 ,UGPF(2) 和 UKF(2) 功能 相近 ,表明 动态 或 测量 模型 的 非 线 性 度 不 足以 
拟 合 状态 矢量 的 概率 密度 ,因为 状态 矢量 密度 通常 为 高 斯 分 布 ,UGPF(2) 的 
RMS 误差 与 UKF(2) 的 RMS 误差 阶 数 相同 。 因 此 判断 ,使 用 粒子 滤波 器 需要 相 
当 大 的 计算 量 ,在 这 个 问题 空间 里 不 必要 的 .如 果 在 实际 使 用 时 遇 到 非 高 斯 测量 
噪声 ,可 能 会 重新 考虑 这 个 判断 。 

与 UKF(1) 和 UKF(2) 相 比 ,三 阶 无 迹 卡 尔 曼 滤波 器 (UKF(3) ) 具有 明显 
的 性 能 问题 ,尽管 与 一 些 参数 类 型 相 比 ,UKF(3) 的 均 方 根 误差 明显 较 好 ,但 在 
某 些 情况 下 它 的 表现 比 UKF(1) 或 UKF(2) 更 差 ,当前 的 三 阶 动态 模型 的 性 能 
需要 进一步 研究 。 

UKF(2) 求解 器 似乎 比 UKF(1) 的 性 能 略 好 ,RMS 统计 数据 大 致 为 位 置 、 角 
度 和 平移 速度 。 

在 角速度 随时 间 的 变化 中 ,UKF(2) 求解 右 的 RMS 误 差 在 大 约 100ms 稳定 ， 
而 UKF(1) 求解 器 则 继续 振荡 。 


19.6.3 结论 与 展望 


在 所 有 求解 器 测试 中 ,二 阶 无 迹 卡尔 曼 滤波 器 (UKF(2) ) 的 性 能 最 好 ,与 标 
EF BT NLLSQ 求解 器 相 比 有 显著 改进 ,尽管 UGPF(2) 粒子 滤波 器 的 性 能 与 其 
相似 ,但 计算 量 非常 大 ,无 法 实际 使 用 。 

在 所 有 阶 数 大 于 零 的 求解 器 测试 中 ,投放 事件 的 冲击 导致 加 速度 跃 变 后 ,性 
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能 马上 下 降 。 由 于 这 种 脉冲 的 动态 模型 会 显著 改善 滤波 器 的 性 能 ,动态 模型 中 的 
自由 落体 方程 不 足以 表现 过 程 ( 如 受 力 函 数 ) 中 的 非 噪声 变化 。 

解决 退化 问题 的 男 一 种 方法 是 直接 测量 导致 性 能 下 降 的 加 速度 ,下 章 将 讨 
论 将 图 像 测量 与 加 速度 计 和 陀螺 仪 的 测量 结合 起 来 。 

拟 研究 的 男 一 种 方法 是 不 仅 使 用 图 像 中 二 维特 征 点 的 位 置 ,还 采取 光 流 技 
术 测 量 图 像 序列 得 二 维 图 像 特征 点 速度 ,在 测量 模型 中 增加 特征 点 图 像 速度 ,有 
助 于 稳定 和 提高 贝 叶 斯 滤波 器 三 维 平移 和 角速度 估计 的 精度 。 


附录 19.A ”四 元 数 、 轴 角 矢 量 与 旋转 


19. A.1 旋转 表示 之 间 的 转换 


ita = [a,,a,,a.]”e R 为 旋转 的 轴 角 表示 , 其 中 a 的 方向 表示 旋转 轴 ， 
la || 表示 旋转 值 (单位 为 弧度 )。 

ita = [q..q.,q,,q.] ”Ee H,H 为 四 元 数 空间 ,是 旋转 的 单位 四 元 数 表示 ,这 
E a, 为 实数 ,g, 4, Ma, 为 纯 虚数 系数 ,有 Vg + 人 +q +q =1。 

SM e R xR 为 3 x 3 旋转 矩阵 ,这 样 M 为 正 交 的 , 且 M =+1。 

为 将 轴 角 矢量 a 变换 为 单位 四 元 数 g ,定义 如 下 函数 Q: R? — A, Bil 








in Wal i 
q = Q,(a) = perm 2 J 2 [ a, ,a, ,a,] ll 4942 
27 Qv 
为 将 单位 四 元 数 q 变换 为 轴 角 矢量 a ,定义 如 下 函数 A, LH. — R° , 即 
2cos” T 
a= A (q) = 14.9, q.) (19 - 162) 
Ji- ` 


为 将 单位 四 元 数 q 变换 为 旋转 矩阵 M ,定义 如 下 函数 AA, :H — R° x R° , B|) 
2(q +q.) -1 2(q4,-4,4.) 2(q.q, “| 





M = M,(q) =| 2(q.q, *q4.) 2(q *4) -1 2(gq,q, - q,q,) 
2(4,4, -4,4,) 2(4,4, +99) 2495 +95) -1 
(19 - 163) 
为 将 轴 角 矢量 a 变换 为 旋转 矩阵 M ,定义 如 下 函数 M,:R — R° x R° Bil 
M = M,(a) = M,(Q,(a)) (19 - 164) 
为 将 欧 拉 角 (由 ,9,p) 变换 为 单位 四 元 数 q ,定义 函数 Q... (6,0,o) :R 为 
Q@puee( 中 ,0,P) = 4,4,4, (19 — 165) 


这 里 q, q, 和 q, 为 表示 欧 拉 角 的 三 个 四 元 数 ,分别 为 
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: 
q, = [cos sin $-,0,0] (19 - 166) 


= loos E " 
q, = | cos 2 ,0 ,sin 2 ,0| (19 = 167) 
* 
l, = | cos $.0,0,sin 2 (19 - 168) 


19.4.2 方位 与 旋转 的 表示 方法 


用 两 个 量 一 起 表示 旋转 ,一 个 是 状态 矢量 中 的 3D 轴 角 矢量 &, 另 一 个 是 状 
态 矢 量 之 外 的 外 部 单位 四 元 数 g, 只 是 在 观测 模型 中 用 到 ,在 迭代 滤波 器 中 ,四 
元 数 g 表 示 此 前 相对 于 参考 坐标 系 的 刚体 方向 ,矢量 a 表示 对 4 的 调整 ,a 只 能 部 
分 表示 当前 方向 。 当 旋转 可 导 时 ,可 用 3D 状态 矢量 来 表示 a ,a ,a ,…。 
两 个 四 元 数 p A g 的 乘积 结果 为 四 元 数 ”", 用 下 式 表示 


r = pq (19 - 169) 
或 者 
i. P. — Px = Py =P.: q. 
r x 5 -= Ps À x 
ra |a 0g (19 - 170) 
n| |» Pp Pp. -py 
r P: = P; p. P, q. 


可 以 写成 矩阵 矢量 的 乘法 , 即 

r = Pq (19 — 171) 

外 部 单位 四 元 数 在 滤波 器 迭代 过 程 中 不 会 改变 ,而 且 只 在 观测 模型 中 使 用 。 

一 旦 完成 滤波 器 更 新 步骤 ,状态 矢量 an, 中 校正 后 的 轴 角 部 分 可 用 于 更 新 外 部 
单位 四 元 数 , 此 后 a,, 重 置 为 

q, = Q,(a,,,)q,-, (19 — 172) 

a,,, *— [0,0,0]" (19 - 173) 


19.4.3 ”点 旋转 与 帧 旋转 


下 面 给 出 点 旋转 和 帧 旋转 的 区 别 。 一 般 来 说 ,在 点 旋转 时 ,点 或 矢量 旋转 而 
轴 的 方向 保持 不 变 ; 而 在 帧 旋转 时 , 轴 旋 转 而 点 或 天 量 保 持 不 变 。 为 将 3D AE v 
旋转 单位 四 元 数 g 所 指定 的 角度 ,用 式 (19 - 164) 中 给 出 的 函数 .M, 构造 一 个 旋 
FEER, BI 


M, = M,(4) (19 - 174) 
进行 乘积 运算 以 得 旋转 矢量 v” 为 
y" = M (19 - 175) 


令 w ”为 一 个 矢量 ,其 元 素 关 于 坐标 帧 用 指定 ,用 qn_, 表示 坐标 系 帧 及 和 
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S2 之 间 的 角度 差 , 为 实现 帧 旋转 ,找到 vu HRF 2 的 坐标 系 , 即 


Mana = Ma da.) (19 - 176) 
e 一 A 
po 2 E a (19 - 177) 


注意 : 帧 旋转 为 点 旋转 的 逆 。 因 为 旋转 矩阵 是 正 交 的 ,如 果 M 为 旋转 矩阵 ， 
WAM" = MERR A MR 的 原点 是 一 致 的 。 
在 观测 模型 中 ,用 两 种 方式 旋转 .中 找到 刚体 上 特征 点 的 坐标 ,将 其 经 过 旋 
转 和 平移 ,变换 到 参考 (飞机 ) 坐标 系 中 物体 的 估计 位 置 .因为 物体 上 的 点 被 旋 
转 、 然 后 平移 到 新 的 位 置 ,这 里 执行 一 个 点 变换 , 即 
s) = M.s+p (19 - 178) 
这 里 M, 为 由 估计 的 物体 旋转 角度 得 的 旋转 矩阵 ,3D 矢量 p 指定 物体 重心 的 平 
移 , 而 s 指 定 3D 特征 点 在 本 体 坐 标 系 中 的 位 置 -@) 利用 帧 变换 ( 负 平移 后 进行 一 
个 帧 旋转 ) 计算 点 s“ 相对 摄像 机 坐标 系 的 位 置 , 即 
ge) = MT (st -p_) (19 - 179) 
WP :M am 表示 摄像 机 的 方向 ;Ps。。 为 其 位 置 。 
注意 :点 变换 的 逆转 换 为 帧 变换 。 所 以 为 了 求 式 (19 - 178) 的 逆 , 需 要 计算 


s = Mi(s'"? _ p) (19 - 180) 
或 者 为 求 式 (19 - 179) 的 逆 , 需 要 计算 
s™ = M sU +P (19 - 181) 
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第 20 章 
用 摄影 测量 和 惯性 测量 进行 传感器 融合 





20.1 EF jÉ 


第 19 章 给 出 了 一 种 跟踪 姿势 轨迹 (位 置 和 方向 的 时 间 函 数 ) 方法 , 仅 使 用 
外 部 的 视频 摄像 传感器 估计 刚性 落体 ,这 种 方法 中 ,用 安装 在 刚体 表面 的 多 个 摄 
像 机 采集 视频 图 像 . 因 此 ,测量 值 集合 仅 包含 摄像 机 中 特征 点 的 位 置 , 某 些 方向 
上 刚体 由 于 特征 点 的 遮挡 ,会 有 数据 的 丢失 ,此 外 ,速度 或 加 速度 测量 值 无 法 直 
接 获 得 ,所 以 与 位 置 与 方向 相关 的 高 阶 导数 项 的 估计 精度 降低 。 

本 章 将 研究 在 物体 内 部 额外 加 装 惯性 传感器 ,包括 多 轴 加 速度 传感器 和 陀 
螺 仪 ,这 二 者 合 在 一 起 ,被 称 为 惯性 测量 单元 (Inertial Measurement Unit,IMU) 。 
IMU 提供 沿 三 个 本 体 坐 标 轴 的 线性 加 速度 和 相对 三 个 本 体 坐标 轴 的 角速度 ,这 
就 需要 将 IMU 测量 数据 时 间 流 与 视频 数据 时 间 流 融合 起 来 ,得 利用 两 种 异步 测 
量 数 据 流 的 跟踪 滤波 器 。 

在 文献 [1 ,2] 中 首次 实现 了 仅 使 用 单个 摄像 机 ( 单 目 成 像 ) 得 特征 点 的 时 
间 连 续 图 像 , 在 此 基础 上 跟踪 刚体 的 方法 ,luandWohn'" 的 论文 中 ,实现 了 一 个 
动态 模型 ,假设 平移 加 速度 和 旋转 加 速度 均 为 常数 ,而 Lee 等 ”提出 一 个 动态 模 
型 ,平移 和 旋转 速度 均 为 常数 ,文献 [3 - 6] 给 出 更 为 先进 的 方法 ,使 用 多 个 摄像 
机 采集 特征 点 图 像 , 在 每 个 时 间 步 计算 刚体 的 姿态 .Gennery ,Huang 和 
Natravali ^ 以 及 Ude ^ 均 使 用 非 线 性 最 小 二 乘法 (NLLSQ) 进行 状态 矢量 估计 ， 
假定 没有 动态 品 声 。 这 种 方法 在 时 间 上 不 递归 , 而 是 在 每 个 时 间 步 进行 迭代 。 
Halvorsen 等 提出 使 用 时 间 上 递归 的 扩展 卡尔 曼 滤波 器 (EKF) ,来 处 理 多 摄像 机 
视频 图 像 特 征 点 缺失 的 问题 。 

在 文献 [8 -12] 中 ,提出 用 IMU 或 磁场 . 角 速 率 和 重力 传感器 ( MARG ) 得 的 
数据 估计 刚体 的 方向 ,在 文献 [8] 中 ,Algrain 和 Samiie 使 用 一 种 加 速度 计 陀螺 
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线性 高 斯 (AGLG ) 估计 技术 ,实质 上 是 一 种 线性 最 小 二 乘 迭 代 方 法 ,没有 时 间 递 
归 。 文 献 [9 - 12] 中 将 MARG 传感器 与 EKF 相 结 合 ,跟踪 刚体 的 方向 ,这 4 篇 论 
文 对 原始 算法 稍 加 修改 ,跟踪 人 体 的 手 或 者 肢体 的 朝向 ,在 时 间 上 递归 ，。 

视频 和 IMU 数据 的 融合 是 文献 [13 -15] 人 研究 的 焦点 ,在 所 有 情况 下 IMU 安 
装 在 摄像 机 的 旁边 ,融合 的 目的 是 为 了 校准 摄像 机 。 因 为 IMU 没有 安装 在 跟踪 
目标 的 外 部 ,在 这 些 文献 提出 的 方法 中 ,IMU 数据 不 能 直接 作为 测量 数据 在 任何 
目标 状态 估计 方法 中 使 用 。 

下 面 提出 一 种 方法 ,包括 一 个 安装 在 刚体 上 的 IMU 和 外 部 摄像 机 以 观测 刚 
体 上 的 特征 点 。 由 安装 在 刚体 上 的 IMU 得 的 数据 与 视频 图 像 数 据 融合 ,生成 异 
步 数 据 的 单一 流 , 用 在 跟踪 算法 的 更 新 步骤 中 。 在 这 个 方法 中 ,采用 无 迹 卡 尔 曼 
滤波 器 (UKF ) 作为 融合 跟踪 算法 ,算法 的 性 能 结果 与 单独 使 用 视频 图 像 或 IMU 
数据 进行 了 比较 。 

图 20 - 1 给 出 了 一 些 IMU 的 图 像 ,独立 的 或 安装 在 物体 上 (类 似 炸弹 的 刚 
体 )。 





图 20 -1 连接 发 射 天 线 的 独立 IMU ,以 及 安装 在 一 个 500 磅 物体 上 的 IMU 
20.2 刚体 运动 过 程 模 型 


第 19.2 节 描述 了 刚体 运动 的 动态 运动 模型 ,建议 读者 参考 第 19 章 中 对 这 些 
动态 模型 的 详细 描述 ,第 19 章 的 结尾 处 ,试验 表明 使 用 二 阶 恒 定 加 速度 可 得 到 
相对 最 好 的 性 能 ,这 里 修改 状态 矢量 ,使 其 包括 控制 平移 和 旋转 运动 的 速度 和 加 
速度 矢量 ,本 章 将 专门 分 析 这 个 模型 ,这 里 的 运动 模型 描述 了 刚体 的 平移 和 旋转 
运动 , 是 时 间 的 函数 。 根据 瞬时 状态 矢量 x(t) = [p(t) ,p(t),p',a' (0, 
a' (t) ,a' ,根据 外 部 单位 四 元 数 q(t) ,定义 刚体 相对 于 参考 笛 卡 儿 帧 的 质心 
的 平移 为 p(t) = (x(t) ,y(t) ,z(1) ] ,本 体 相 对 重心 的 方向 由 轴 角 矢量 a(1) = 
[a,(t),a (t),a,(t)]' 与 外 部 单位 四 元 数 q(t) = (9, (0,9, (0,4, (0 ,gq.(t)]" 
相 结 合 给 出 。 

一 般 来 说 ,可 以 将 状态 矢量 时 间 转 移 方程 写 为 
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x, = fna) ua (20 - 1) 
这 里 v, , 为 导致 过 程 不 确定 性 的 噪声 ,在 所 有 的 情况 下 都 假设 噪声 是 高 斯 加 性 
噪声 。 


20.3 ”传感器 融合 观测 模型 


定义 将 状态 矢量 x, 变换 至 传感器 融合 观测 矢量 z, 的 分 量 的 函数 ,如 果 包 含 
观测 噪声 ,此 变换 可 写成 
z, = h,(x,) + w, (20 - 2) 
iX iw, ~ COR.) -观测 值 由 两 个 独立 的 分 量 构成 ,IMU 分 量 和 摄影 测量 分 量 ， 
必须 融合 在 一 起 来 提供 观测 数据 的 异步 流 。 


20.3.1 观测 模型 的 惯性 测量 装置 元 件 


由 3 个 正 交 加 速 计 和 3 个 正 交 速 率 陀螺 仪 得 IMU 输 出 数据 ,t, 时 刻 的 IMU 观 
测 矢量 可 写成 
Kuna = D usos Sin iaa] j (20 - 3) 
这 里 z., ,z,, Alz.,, 为 加 速度 (用 C 表示 ) ,分 别 用 x - 向 前 \y — eA Az — 向 下 
加 速度 计 进行 测量 ;而 z,, ,zo,, 和 z。,。 为 旋转 速率 (单位 为 度 ) ,分 别 用 翻滚 (相对 
x 轴 逆 时 针 旋 转 ) 俯仰 (相对 y 轴 逆 时 针 旋 转 ) 和 偏转 (相对 z 轴 逆 时 针 旋 转 ) 速 
率 陀螺 仪 测量 ,注意 ,因为 IMU 安装 到 刚体 上 , 当 刚 体 旋 转 时 ,IMU 随 之 进行 旋 
转 , 这 样 zw 的 测量 值 是 关于 刚体 的 本 体 坐 标 系 , 而 不 是 关于 参考 (飞机 ) 坐 
标 系 。 
可 以 根据 状态 矢量 写 出 IMU 观测 矢量 ,通过 转换 方程 BI 
Zi, = huu, (Xn) + Winun (20 - 4) 
这 里 wio ~ NCO Rin.) FF Rua, 定义 为 由 加 速度 计 和 速率 陀螺 仪 的 测量 值 
方差 构成 的 对 角 品 声 协 方差 矩阵 , 即 
Tcl, 0; 


Ryo, = | : (20 - 5) 
0, Tonks 

XP io, Lo, 分 别 为 加 速度 计 标 准 偏差 和 角 速 率 测 量 值 标准 偏差 ;到 为 3 x 3 

单位 矩阵 ;0; 为 3 x 3 FE, 

为 构造 himu. (x, ) ,首先 将 ge = E SE E 定义 为 在 参考 坐标 系 的 
环境 加 速度 (用 G 表 示 ) ,地 球 表面 的 重力 矢量 为 gr = [0,0, -1] 。 但 由 于 参考 
的 飞机 为 机 动 的 ,并 且 有 荷载 6,g, 将 随时 间 变 化 。 但 是 由 于 飞机 比 刚体 大 很 
多 ,因此 有 更 大 的 惯性 ,这 里 选取 gg 为 常数 ,因为 与 跟踪 的 刚体 相 比 , 它 随 时 间 
的 变化 非常 缓慢 ,此 外 ,还 忽略 由 飞机 机 动 性 引起 的 参考 坐标 系 在 方向 上 的 改 
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变 。 在 后 续 实 现 中 ,可 以 通过 考虑 这 些微 小 的 变化 来 修改 这 一 过 程 。 还 假设 IMU 
置 于 刚体 坐标 系 的 中 心 (通常 为 其 重心 ) ,应 该 加 入 一 个 杆 辟 校正 ,以 弥补 加 速 
度 计 绕 刚 体重 心 旋转 而 引起 的 向 心力 和 切 向 线性 加 速度 。 

旋转 和 矩阵 Mw 表示 刚体 相对 参考 坐标 系 的 当前 方向 ,可 用 与 第 19 章 中 观测 
模型 相同 的 方式 构造 ,利用 式 (19 - 48) ~ 式 (19 - 50) ,得 

M, = M, ` M, 

M, = M,(4,) 

M, , = Mla) (20 - 6) 

H T z, 的 测量 值 是 相对 于 本 体 坐 标 系 的 ,可 用 下 式 生成 无 噪 本 体 参 考 旋 转速 率 


2d,ny2b,n 和 Zono Bp 
Zp,n 
Ë 
z 


用 xy 和 z 加 速度 计 (在 本 体 一 参考 坐标 系 ) fE t, 时 刻 进行 测量 ,得 到 3D 加 
速度 为 


E = ML, (20 -7) 





i” US ter (20 - 8) 
式 中 :p。 ”是 刚体 重心 的 平移 加 速度 ;gw"”” 为 将 参考 坐标 系 旋转 至 本 体 坐标 
系 所 得 的 加 速度 。 这 样 p,” + grat”) 可 简单 地 由 下 式 求 得 , 即 


po + ge = MiB, + Mig. 
= M, + gi) (20 -9) 
无 噪 本 体 参 考 加 速度 测量 可 以 表示 为 
z n 
图 2g = MLD, + Bua) (20 - 10) 


利用 式 (20 - 7) ~ 式 (20 - 10) ,可 将 hw , (x,) 表示 为 


Es T 
hy, (x,) US [£,, ?yn z,n 2 4.n ?Zn ?pn ] 
(body) 
En 


= T d 
at" y) 


zn 


(20 - 11) 


20.3.2 ”观测 模型 的 摄影 测量 装置 


这 里 简单 回顾 第 19. 3 节 中 观测 模型 的 定义 ,来 定义 观测 模型 的 摄影 测量 分 
Ht ORES 19 章 中 的 分 量 重新 命名 为 
^md aaa (20 - 12) 
h,(x,) — hu, (Xn) (20 - 13) 
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W, —* W hoo,n (20 - 14) 

R, — Raw» (20 - 15) 
和 第 19 REZ {E,Z photon hua... (Xn) F wu... 的 维 数 是 2M, ,而 R... 的 维 数 为 
2M, x2M, ,这 里 MM, 为 所 有 摄像 机 中 可 见 特征 点 的 数目 。h,,,, 和 AM 是 时 间 无 关 
的 ,因为 每 个 摄像 机 可 观测 到 的 特征 点 数目 可 以 随时 间 变 化 。 


20.3.3 组合 传感器 融合 观测 模型 


为 指明 c, 时 刻 可 获得 哪 种 类 型 的 测量 值 , 定 义 一 个 观测 标志 位 F, 为 
1 只 提供 IMU 数据 
F, = 2 只 提供 摄影 测量 数据 (20 - 16) 
3 提供 两 种 类 型 数据 


t, 时 刻 的 观测 矢量 z, 可 写成 


ZIMU,n F, = 
Z, = | Za F, = 2 (20 - 17) 
~ AN F =3 
同样 ,变换 函数 h,(x,) 可 写成 
huu,.( x, ) F, 21 
h,(x,) = | hao. (X, ) F, = 2 (20 - 18) 
[pa (X, ) Aaa 21^ F, =3 


作为 其 分 量 的 组 合 ,可 将 观测 噪声 假定 为 零 均 值 高 斯 过 程 w, ~ N(O,R,), 
其 中 R, 定义 为 对 角 噪 声 协 方差 矩阵 , 即 


R mu F, = 
; P" F = 2 
R. = i (20 - 19) 
Ps Osr | F, 23 
05, R so 


这 里 Osr 为 零 矩阵 的 维 数 6 x 2M, 0 
如 果 IMU 时 钟 与 摄影 测量 时 钟 不 同步 ,来 自 IMU 的 数据 可 能 与 摄影 测量 数 
据 异 步 , 连 续 测量 值 之 间 的 时 间 差 可 能 会 不 一 致 。 因 此 ,在 所 有 跟踪 滤波 器 中 必 
须 用 时 间 相 关 的 T, ,定义 为 
T, = t, ~ Ên (20 - 20) 


20.4 生成 模拟 数据 
为 了 评估 第 19 章 中 给 出 各 种 跟踪 方法 的 性 能 ,提出 一 种 模拟 刚体 ,并 生成 


一 个 模拟 航 迹 来 代表 投放 弹药 的 事件 .可 以 由 模拟 的 ”真实 ” 航 迹 生成 由 一 组 虚 
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拟 摄像 机 看 到 的 模拟 含 品 观 测 值 的 蒙特 卡 洛 集合 .蒙特 卡 洛 模 拟 测 量 集合 被 用 
于 各 种 各 样 的 估计 (求解 器 ) 方法 ,用 每 种 估计 算法 得 的 跟踪 误差 均 方 根 (RMS ) 
进行 性 能 的 比较 分 析 。 

这 里 重用 第 19 章 中 的 模拟 航 迹 和 蒙特 卡 洛 含 噪 摄 像 机 观测 值 集合 ,在 含 品 
测量 值 中 添加 安装 在 模拟 刚体 上 的 虚拟 IMU 的 加 速度 计 和 速率 陀螺 仪 的 测量 
值 ,因此 ,将 进行 各 种 估计 方法 的 比较 分 析 , 并 分 析 增 加 IMU 测量 值得 的 性 能 
改进 。 

20. 4.1 模拟 轨迹 


这 里 使 用 与 第 19. 5.2 节 相 同 的 模拟 航 迹 ,唯一 不 同 的 是 生成 数据 速率 更 快 
的 时 间 上 的 序列 , 即 
t= |t, 0.001*n n =0,1,2,---,1000} (20 - 21) 
尽管 IMU 测量 值 的 生成 周期 更 短 ,为 0.001s, 图 像 测量 值 仍 采用 较 慢 的 0. 005s 
的 周期 ,为 指出 这 一 点 ,将 标记 变量 F, 设置 为 
3 {n,mod5} = 0 
F, = Í 
l 其 他 
根据 式 (19 - 106) ~ 式 (19 - 111) 可 构造 模拟 航 迹 。 


20. 4.2 ”模拟 摄像 机 
按照 第 19. 5. 3 节 构 造 两 个 虚拟 摄像 机 。 
20.4.3 ”模拟 测量 值 


(20 - 22) 


20.4.3.1 IMU 测量 值 


对 于 每 个 n = |0,1,2,…,1000| ,生成 IMU 模拟 测量 值 矢量 zo uus ,定义 

为 
EMU tk = himu.n (X, ae) 十 W IMU,n,synth (20 - 23) 
Wimu,n.synth ~~ NCO , Ry syn) (20 - 24) 


2 
LR E 0, 


R imu ,synih - | (20 - 25) 


2 
0, Ol 


DOP io, 和 oo, 分 别 为 0.1G Fl 5. 0°/s., 
20. 4.3.2 ”摄像 机 测量 值 


在 时 间 序 列 中 , 取 每 第 5 个 点 作为 摄像 机 的 测量 值 ,对 于 每 个 n = 10,1， 
2 ,… ,200| 生成 相机 的 模拟 测量 矢量 etium AE LN 
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a ES B aim So Fn keo ) * W Shoto, Sn , synth (20 on 26) 
W boto, Sn, synth kd NO s ae So ih ) (20 = 27 ) 
R po -— = 0 olas, = Canidae (20 = 28 ) 


XP :0 pixel 为 2 像素 。 
20.4.3.3 ”组 合 测量 值 
对 于 每 个 n = 10,1,2,--,1000] ,生成 组 合 模拟 测量 值 矢量 zw ,定义 为 


`. = | k. (zas) + ron tm smods} =0 (oS 295 
hua... (X, true) * W, systh 其 他 
b, aUos) A [Nan Rs, >) sn (x...) 1° (20 =y 30) 
W, syah es N(0,R, su) (20 EE; 31 ) 
Ris ayn Üs, 
| =e SF ia mods] -0 
E, sith = Osr Rua saan (20 = 32) 
R imu synth 其 他 


AP Osr 为 6 x 32 维 的 零 矩阵 注意, 在 每 个 时 间 步 进行 一 个 新 的 蒙特 卡 洛 采 样 
R 


n,synth 9 


20.5 ” 估 值 法 


在 传感器 融合 模型 比较 分 析 的 估计 方法 包括 : 仅 使 用 摄影 测量 数据 的 方法 、 
仅 使 用 IMU 数据 的 方法 以 及 将 IMU 数据 和 视频 数据 相 融合 的 方法 。 第 19 章 得 出 
结论 ,二 阶 无 迹 卡 尔 曼 滤波 器 (UKF(2) ) 在 所 有 测试 的 求解 器 当中 性 能 最 好 , 当 
仅 使 用 摄影 测量 视频 数据 时 ,UKF(2) 相对 于 标准 零 阶 NLLSQ 求解 器 有 很 大 改 
进 。 为 完整 起 见 ,第 20.6 节 性 能 比较 分 析 将 包括 只 使 用 摄影 测量 观测 数据 时 
UKF(2) 和 NLLSQ 性 能 结果 。 当 只 用 IMU 数据 作为 观测 值 时 ,用 二 阶 预测 - TE 
正方 法 并 用 UKF(2) 作为 跟踪 滤波 器 ,为 实现 这 一 点 , 设 F, = 1, Vn RRA 
IMU 数据 可 用 作 观 测 值 。 

当 仅 使 用 摄影 测量 视频 数据 时 ,NLLSQ 和 UKF(2) 估计 过 程 的 细节 可 分 别 
参考 第 19. 4. 1 节 和 第 19. 4. 2 节 。 此 处 将 不 再 赣 述 这 些 方法 的 细节 。 第 20. 5. 1 节 
将 讨论 仅 使 用 IMU 数据 时 的 预测 - 校正 估计 算法 。 


20.5.1 惯性 测量 装置 数据 初始 化 


在 通用 的 刚体 位 置 及 方向 的 估计 方法 中 ,使 用 IMU 采集 的 平移 加 速度 和 角 
速率 测量 值 , 利用 初 值 问题 (IVP) 求解 器 对 常 微分 方程 组 (ODE) 进行 数值 求 
解 有 和 多 种 可 选 的 IVP 求 解 器 ,这 里 详细 介绍 一 种 简单 精确 的 欧 拉 梯 形 或 称 为 预 
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测 — 校正 方法 的 和 迭代 求解 器 。 首 先 构造 一 个 一 阶 常 微分 方程 组 ,与 刚体 的 
IMU 测量 值 中 位 置 .方向 和 平移 速度 相关 ;然后 ,简要 描述 如 何 对 求解 器 进行 初 
始 化 ,以 及 如 何 使 用 它 来 求解 常 微 分 方程 组 ,以 得 目标 航 迹 (位 置 和 方向 ) 的 时 
间 函 数 估计 。 


20.5.1.1 预测 一 一 校正 IVP 求解 器 


通常 ,可 将 一 阶 常 微分 方程 组 求解 看 成 一 个 初 值 问题 .已 知 初始 值 x(t) = 

x, ,由 矢量 函数 Fi,z(t) ) 来 估计 未 知 函数 x(1) ox (2) 与 其 一 阶 时 间 导 数 关系 可 
写成 

dt = f(t,x(t)) (20 - 33) 


x(t) = X, (20 - 34) 
欧 拉 方法 是 最 简单 的 求 常 微分 方程 积分 的 正 交规 则 。 令 t = [tn = 0,1, 
2,…,N| SHR x, = x(4,)。 利 用 欧 拉 法 则 ,x, 的 预测 值 可 由 下 式 得 , 即 
a" oux. TII) (20 - 35) 
这 里 7 = t, — t, KKALI RUE OE E BU) HUU , 仅 在 区 域 的 左 端点 对 被 积 函 
数 进行 估计 。 当 f(t,x) 为 常量 时 ,这 一 规则 是 精确 的 ;但 当 .F(t,z) 为 线性 时 ,这 
一 规则 不 精确 ,其 误差 与 步 长 了 的 大 小 成 正比 。 为 得 准确 的 估计 ,需要 选取 小 的 
步 长 ,但 这 里 无 法 实现 根据 预计 误差 要 求 以 自动 的 方式 来 确定 步 长 ,为 减少 误差 
的 不 确定 性 ,有 必要 在 欧 拉 步 又 后 ,在 区 域 的 右 端点 加 上 第 二 个 函数 估计 。 这 种 
方法 称 为 预测 - 校正 方法 ,以 迭代 方式 使 用 梯形 法 则 ,首先 ,使 用 式 (20 - 35), 
对 整个 区 域 执行 欧 拉 步骤 ,得 初 值 预测 ;然后 , 求 外 推 点 的 函数 值 xz” , 取 两 个 斜 
率 的 均值 ,得 x, 的 第 一 个 校正 估计 , 即 


Š. = tS ter) AAC] (20-36) 


对 于 k > 1 ,校正 步骤 是 迭代 重复 的 ,x 的 估计 质量 用 当前 估计 与 上 一 个 估计 的 
比较 实现 , 即 


e, = | Sya 一 Sia l° (20 - 37) 
Ban = xa + Etter) + Baad] — (0-38) 
当 SEENE, Be, < o REER Ke = bmn JEP (ema = 


2.2204 x 10° 是 大 于 零 的 最 小 双 精 度 值 ,如 果 加 1, 则 得 可 区 分 的 大 于 1 的 数 
值 , 当 和 迭代 达到 阅 值 ,设置 x，= x, ,然后 进行 下 一 步 。 


20. 5. 1.2 IMU 问题 的 常 微分 方程 组 


现在 构造 一 个 一 阶 常 微分 方程 组 ,将 本 体 - 参考 坐标 系 下 IMU 测量 得 的 旋 
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转速 率 与 参考 坐标 系 下 的 旋转 角度 和 速度 联系 起 来 ,并 将 本 体 - 参考 坐标 系 下 
的 平移 加 速度 与 飞机 坐标 系 下 的 速度 和 位 置 联系 起 来 , 令 z” = [(z2)",(z?)"]" 
Jy di IMU 得 的 测量 值 ,这 里 ze = [z, ,z,,2,]" 表示 3 个 正 交 加 速度 计 得 的 测量 值 ， 
Zz) = [z520] 表示 3 个 正 交 速 率 陀螺 仪 得 的 测量 值 . 令 x = [p'.p',q']' 为 待 
估计 的 未 知 状态 矢量 ,其 中 p = p.p, p]! 表示 3 维 位 置 ,p = [p, ,p,,p;] 表示 
3 维 速度 ,4 = [9g,,q, ,9, ,9:]" 是 一 个 单位 四 元 数 ,表示 3 维 方 向 ,注意 z? 在 本 体 - 
参考 坐标 系 中 测量 ,而 x 在 参考 坐标 系 中 表示 。 
为 得 4 的 常 微分 方程 ,从 式 (19 - 122) 开始 ,并 用 zo 替换 w, 得 


— = 2 gz, (20 — 39) 


其 中 四 元 数 乘法 4 z。 中 ,被 乘 数 z。 用 纯 四 元 数 [0,z%] = [0,2,,2,,2,]" 来 
表示 。 
同样 ,为 得 到 3 维 位 置 p 与 IMU 测 得 的 本 体 - 参考 加 速度 测量 值 z? 之 间 关 

系 , 可 通过 二 阶 常 微分 方程 得 , 即 
dp 
dt? 

这 里 旋转 矩阵 M, = M,(q) 将 zo? 旋转 至 参考 坐标 系 ,g 为 参考 坐标 系统 中 的 重 

力 加 速度 。 二 阶 方程 式 (20 - 40) 可 以 通过 加 上 一 个 速度 方程 ,简化 为 一 个 一 阶 

方程 组 , 即 


= Mai -g (20 - 40) 


H =p (20 - 41) 
利用 式 (20 - 41) ,可 将 式 (20 - 40) 重 写 为 
dp = Mr (20 - 42) 


dt 
这 样 , 式 (20 - 39) \ 式 (20 - 41) 和 式 (20 - 42) 构成 一 个 10 维 的 一 阶 常 微分 方 
程 组 ,可 充分 描述 IMU 测量 值 与 刚体 的 位 置 方向 和 平移 速度 之 间 的 关系 。 如 果 
定义 x = [p.p q ] ,可 将 式 (20 - 33) 写成 








p p 
n = f(t,x) ze = | MZ: -g (20 - 43) 
q lel 


20.5.1.3 ”初始 化 并 使 用 预测 - 校正 器 对 位 置 和 方向 进行 估计 


人 - 校正 器 进行 初始 化 , 即 
= [PoP] 
= [0,0,0,0,0,0,1,0,0,0]' (20 - 44) 
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其 中 最 后 四 个 元 素 是 单位 四 元 数 ,表示 零 旋 转 。 注 意 在 其 它 情况 下 xo 可 能 包含 
非 零 元 素 ,这 取决 于 环境 。 由 于 预测 - 校正 器 是 一 个 非 约束 的 估计 器 ,一 旦 迭代 
生成 了 4 时刻 x, 的 估计 ,四 元 数 的 元 素 q, 不 一 定 是 单位 的 ,因此 不 能 正确 地 表 
示 方 向 。 
为 校正 这 一 问题 ,在 进行 下 一 次 迭代 之 前 对 q, 进行 规范 化 ,将 其 规范 至 至 单位 
长 度 , 即 





(20 - 45) 


20.6 ”性 能 对 比分 析 


用 第 20.4 节 给 出 的 模拟 试验 数据 ,进行 以 下 性 能 分 析 。 注 意 ,和 第 19.6 节 用 
到 的 模拟 航 迹 是 相同 的 ( 图像 数据 具有 相同 的 噪声 ) 。 这 里 对 传感器 融合 估计 滤 
波 器 (UKF(2) ) 的 性 能 进行 比较 ,对 两 种 仅 使 用 摄影 测量 数据 的 估计 器 ( NLLSQ 
和 UKF(2) ) 以 及 两 种 仅 使 用 IMU 数据 的 估计 器 (预测 - 校正 器 和 UKF(2) ) H 
行 对 比 。 在 所 有 情况 下 所 使 用 的 性 能 指标 都 是 均 方 根 (RMS) 误差 ,下 面 将 会 给 
出 ,传感器 融合 估计 器 的 均 方 根 误差 最 小 ,因此 是 最 好 的 估计 器 。 

图 20 -2 ~ 图 20 -4 给 出 一 次 运行 的 估计 轨迹 分 量 中 ,传感器 融合 滤波 器 
与 真实 跟踪 分 量 的 对 比 。 图 20 -5 给 出 了 横向 位 置 真实 航 迹 一 小 部 分 的 特写 ,由 
多 个 估计 器 得 的 估计 航 迹 从 加 在 真实 航 迹 上 。 所 有 估计 算法 用 相同 的 含 噪 观 测 
值 和 初始 值 作为 输入 。 通 过 图 表 分 析 , 可 得 到 以 下 观察 结果 。 

仅 使 用 IMU 数据 情况 下 ,两 种 估计 方法 都 是 不 准确 的 ,尤其 是 UKF。 众 所 周 
知 , 仅 使 用 IMU 数据 的 估计 器 对 初始 条 件 和 校准 偏差 很 敏感 ,但 在 这 种 (模拟 ) 
情况 下 ,初始 条 件 和 校准 是 精确 的 .而 且 , 由 于 观测 值 随 机 噪声 漂移 ,使 得 估计 不 
准确 ,尤其 当 求解 器 的 估计 值 一 个 太 大 而 另 一 个 很 小 时 。 

仅 使 用 摄影 测量 数据 时 ,两 种 估计 都 是 准确 的 ,但 并 不 精确 -UKF(2) 似乎 
HE NLLSQ 更 为 准确 (也 更 精确 ) ,主要 是 由 于 其 状态 矢量 的 估计 中 包含 导数 项 。 
得 出 以 下 结论 :在 所 有 测试 的 估计 方法 中 ,传感器 融合 的 结果 是 最 准确 的 ,因为 
仅 使 用 摄影 测量 的 估计 器 测量 空间 参数 ,对 于 导数 只 能 估计 。 而 仅 使 用 IMU 的 
估计 器 测量 导数 ,必须 估计 空间 参数 ,融合 估计 器 ,以 空间 和 导数 测量 值 为 基础 
进行 估计 。 

由 图 20 - 5 可 很 容易 观察 到 传感器 融合 估计 器 的 性 能 ,将 其 航 迹 与 仅 使 用 
摄影 测量 法 的 UKF HHBE, HH IMU 数据 采样 率 的 175 进行 采样 ,在 摄影 测量 观测 
值 到 达 前 ,传感器 融合 的 趋势 很 平稳 ,而 在 每 个 第 五 采样 点 ,向 一 个 或 另 一 个 方 
向 移动 -. 它 沿 着 与 仅 使 用 摄影 测量 的 UKF 相同 的 方向 移动 ,但 由 于 IMU 数据 提 
供 了 更 准确 的 导数 估计 ,移动 数量 更 小 。 
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图 20 -4 使 用 传感器 融合 估计 器 得 的 平移 和 旋转 加 速度 
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图 20 -5 由 多 个 估计 过 滤器 使 用 相同 的 输入 测量 值得 的 横向 位 置 估计 
这 些 初 步 结论 将 在 第 20. 6. 2 节 进 行 检 验 , 那 里 将 给 出 各 种 估计 器 的 均 方 根 
误差 比较 。 
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20.6.1 滤波 器 性 能 对 比方 法 学 


第 19. 6. 1 节 提出 一 种 性 能 评价 方法 ,利用 均 方 根 误差 衡量 性 能 ,这 里 再 次 
使 用 该 方法 。 同 样 ,采用 第 19. 5 节 提出 的 模拟 无 噪 航 迹 |x,,n = 0,1,2,…,N| 进 
行 分 析 , 而 N = 1000 是 第 20. 4. 1 节 提出 的 首先 将 航 迹 变换 为 一 组 无 噪 的 模拟 
真实 观测 值 |h,(x,) ,mn = 0,1,2,…,N| ,这 里 h,(x,) 可 包含 摄影 测量 法 或 IMU 
测量 值 ,也 可 二 者 同时 包含 ,用 标记 位 F, 来 进行 不 同情 况 的 区 分 -Naws = 100 的 
观测 噪声 集合 |zo,,n = 0,1,2,…,N;r = 1,2, Nas | 是 通过 在 模拟 真实 观测 
值 中 加 入 独立 零 均值 高 斯 噪声 产生 的 Nans 含 噪 观测 值 集合 。 每 个 跟踪 滤波 器 用 
一 个 含 噪 观测 值 集合 用 来 训练 ,以 生成 Was 状态 矢量 估计 集合 |x,,,,n = 0,1, 
2,…,Nir = 1,2,…,Naws| 计算 每 个 跟踪 滤波 器 的 均 方 根 误差 相对 时 间 的 函 
数 ,得 3D 平移 和 角度 位 置 误差 elo A es 3D 平移 和 角速度 误差 ea" 和 
ef? 以 及 3D 平移 和 角 加 速度 误差 ez”” 和 ez 。 


20.6.2 滤波 器 对 比 结果 


对 于 每 个 类 型 的 估计 器 ,计算 均 方 根 误差 关于 时 间 的 函数 ,然后 得 出 每 种 情 
况 的 平均 和 最 大 误差 (一 段 时 间 内 ) 。 在 表 20 - 1. 20 -2 和 表 20 -3 中 给 出 
了 这 些 平均 和 最 大 值 的 概括 总 结 , 分别 对 应 位 置 .速度 和 加 速度 均 方 根 误差 。 
表 20 -1 模拟 数据 RMS 位置 误 差 


NLLSQ 
UKF(2) 
Pred/Corr 
UKF(2) 
UKF(2) 





表 20 -2 模拟 数据 RMS 速度 误差 


NLLSQ N/A N/A N/A N/A 
UKF(2) 


Pred/ Corr 
UKF(2) 
UKF(2) 
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表 20 -3 模拟 数据 RMS 加 速度 误差 


平移 加 速度 人 g 角 加 速度 /((")《sz ) 
数据 类 型 
N/A N/A N/A N/A 


NLLSQ 
UKF(2) 
Pred/Corr 


UKF(2) 





UKF(2) 


图 20 -6,H20 -7 和 图 20 -8 分 别 给 出 五 种 测试 估计 器 中 的 每 一 种 的 时 
程 :位 置 和 旋转 RMS 误差 ,位置 和 旋转 速度 RMS 误差 以 及 位 置 和 旋转 加 速度 
RMS 误差 。 由 表 20 -1~ 表 20 -3 和 图 20 -6 ~ 图 20 -8 可 见 , 除 旋转 加 速度 
情况 以 外 ,使 用 摄影 测量 和 IMU 传感器 数据 融合 的 UKF (2) 估计 器 的 性 能 
最 好 。 
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图 20 -6 所 有 跟踪 滤波 器 使 用 模拟 数据 得 的 位 置 和 方向 的 RMS 误差 
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图 20 -8 所 有 跟踪 滤波 器 使 用 模拟 数据 得 的 位 置 和 方向 加 速度 的 RMS 误差 
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20.7 4& it 


本 章 已 经 证 明了 传感器 融合 估计 器 得 的 估计 结果 比 单独 基于 摄影 测量 或 基 
T IMU 的 估计 更 为 准确 。 在 UKF 求解 器 中 ,两 种 测量 值 互 为 补充 。 

IMU 测量 值 减轻 摄影 测量 数据 中 的 噪声 ,从 而 使 位 置 和 角度 更 为 准确 ;而 
摄影 测量 值 补偿 了 IMU 测量 值 的 偏差 和 漂移 ,使 平移 速度 更 为 准确 。 


20.8 E 8B 


当前 的 测量 模型 假定 IMU 安装 在 刚体 坐标 系 重心 (CG) 。 为 增加 传感器 融 
合 估计 的 实用 性 ,应 该 在 测量 模型 中 增加 杆 臂 校正 ,以 弥补 加 速度 计 中 由 刚体 重 
心 旋转 所 导致 的 向 心力 和 切 向 线性 加 速度 。 本 章 还 试图 证 明 , 使 用 传感器 融合 
分 析 摄 影 测量 和 IMU 数据 ,会 提高 容错 ,一 种 数据 源 中 漏 掉 的 部 分 将 会 被 另外 
数据 源 的 测量 值 所 弥补 。 

除了 可 提高 精度 和 容错 性 ,传感器 融合 还 可 以 提高 摄影 测量 周转 时 间 。 
在 摄影 测量 数据 简化 过 程 中 ,二 维 跟 踪 数 据 的 收集 是 最 耗 时 的 一 步 。 本 章 试 
图 通过 使 用 传感器 融合 重新 组 织 这 一 过 程 ,以 减少 为 得 足够 精确 的 6 自由 度 
解 所 需 的 图 像 测量 值 的 数量 。UKF 的 不 确定 性 模型 提供 了 特定 的 事件 ,对 6 
自由 度 解 的 准确 性 进行 衡量 。 可 以 将 传感器 融合 结果 的 不 确定 性 与 仅 采 用 摄 
影 测量 的 结果 进行 比较 ,以 确定 能 获得 足够 的 精度 情况 下 ,可 收集 的 最 少 的 摄 
影 测量 数据 。 

尽管 当前 IMU 中 速率 陀螺 仪 似乎 很 准确 ,但 加 速度 计 的 校准 偏差 却 很 大 ， 
会 导致 6 自由 度 解 的 严重 错误 。 为 进一步 提高 精度 ,使 用 传感器 融合 来 估计 校 
准 偏差 。 同 时 ,假定 在 物体 与 飞机 距离 很 近 的 短暂 时 间 内 ,飞机 所 承受 的 力 为 常 
数 。 结 合 飞 机 INS 系统 数据 ,来 确定 校正 飞机 的 加 速度 和 旋转 速率 的 变化 是 否 
会 导致 结果 明显 的 差异 。 

本 章 还 试图 解决 IMU 数据 延迟 的 问题 。 用 于 摄影 测量 的 图 像 是 带 有 收集 
和 记录 到 飞机 上 的 时 间 标 记 的 ,IMU 数据 从 目标 传输 到 地 面 站 ,在 加 上 时 间 标 
记 之 前 ,都 收集 在 一 个 数据 缓冲 之 中 。 这 些 多 重 数据 通道 导致 不 确定 长 度 的 时 
间 同 步 误差 。 当 允许 不 同 延迟 时 ,本 章 试图 通过 检查 传感器 融合 解 的 残留 误差 
来 估计 延迟 。 


参考 文献 


[1] lu SL, Wohn K. Estimation of General Rigid Body Motion from a long Sequence of Images. Technical Report , 
Department of Computer Science , University of Pennsylvania, 1990. 
[2] Lee JW,Kim MS, Kweon IS. A Kalman filter based visual tracking algorithm for an object moving in 3D. 
IROS’ 95 Proceedings International Conference on Intelligent Robots and Systems ,1995 , 1. 
[3] Gennery DB. Visual tracking of known three — dimensional objects. Int. J. Comput. Vision ,1992 ,7 (3) :243 - 
270. 
[4] Huang TS, Netravali AN. Motion and structure from feature correspondences; a review. Proc. IEEE ,1994 , 82 
(2) :252 - 268. 
[5] Ude A. Filtering in a unit quaternion space for model — based object tracking. Robot. Au — tonom. Syst, 1999, 
20:163 - 172. 
[6] Halvorsen K, Soderstrom T, Stokes V, and Lanshammer H. Using an extended Kalman filter for rigid body 
pose estimation. J. Biomech. Eng ,2005 ,127 :475 — 483. 
[7] Algrain MC, Saniie J. Estimation of 3D Angular motion using gyroscopes and linear accelerometers. IEEE 
Trans. Aero. Electron. Syst, 1991 ,27(6) :910 - 920. 
[8] Marins JL, Yun X, Bachmann ER ,et al. An extended Kalman filter for quaternion — based orientation estima- 
tion using MARG Sensors. In Proceedings of the 2001 International Conference on Intelligent Robots and Sys- 
tems ,2001 :2003 — 2011. 
[9] Yun X, Lizarraga M, Bachmann ER ,et al. An improved quaternion — based filter for real ~ time tracking of 
rigid body orientation. In Proceedings of the 2003 International Conference on Intelligent Robots and Systems , 
2003 ; 1074 — 1079. 
[10] Sabatini AM. Quaternion — based extended Kalman filter for determining orientation by inertial and magnetic 
sensing. IEEE Trans. Biomech. Eng ,2006 ,53(7 ) :1346 - 1356. 

[11] Yun X, Bachmann ER. Design , implementation , and experimental results of a quaternion — based Kalman fil- 
ter for human body motion Tracking. JEEE Trans. Robot ,2006 ,22(6) :1216 — 1227. 

[12] Kelly J, Sukhatme GS. Fast relative pose calibration for visual and inertial sensors. Proceedings of the 11th 
International Symposium on Experimental Robotics ( ROBIO' 08 ) ,2008. E 

[13] Bleser G and Strieker D. Advanced tracking through efficient image processing and visual — inertial sensor 
fusion. Comput. Graphics ,2009 ,33 :59 — 72. 

[14] Jeon S, Tomizuka M, Katou T. Kinematic Kalman filter ( KKF) for robot end — effector sensing. J. Dyn. 
Syst. Meas. Control ,2009 ,021010 :8. 

[15] Moler C. Numerical Computing with Matlab. 2nd ed. SIAM ,2004. 

[16] Van Loan CF. Introduction to Scientific Computing. 2nd ed. Prentice Hall ,2000. 


302 


